int accly = 1;
int acclz = 2;
int gyrox = 3;

int voltsaccly;
int voltsacclz;
int voltsgyrox;

float gaccly;
float gaccly2;
float gacclz;
float gacclz2;

int ggyrox;

float GYRO_X_INIT_VAL = 423.0;
//--------------------------------------
const float INIT_ANGLE = 0.05; //Adjust this number to your balance point

const int ANGLE_BUFFER_LENGTH = 1;  //  
//-------------------------------------
const float SumErrMax = 20;
const float SumErrMin = -20;

//Tuning Knobs
const int analogInPin1 = 0;  // Analog input pin that the potentiometer is attached to
const int analogInPin2 = 4;  // Analog input pin that the potentiometer is attached to
const int analogInPin3 = 5;  // Analog input pin that the potentiometer is attached to

int sensorValue1 = 0;        // value read from the pot
int sensorValue2 = 0;        // value read from the pot
int sensorValue3 = 0;        // value read from the pot
int outputValue1 = 0;        // value output to the P
int outputValue2 = 0;        // value output to the I
int outputValue3 = 0;        // value output to the D

// <editor-fold defaultstate="collapsed" desc="Global Variables">

int VxAcc = 0, VyAcc = 0, VzAcc = 0;
int VxGyro = 0, VzGyro = 0;
float degX = 0.0f, degY = 0.0f, degZ = 0.0f;

float throttle = 0.0;

float accAngle = 0.0, currAngle = 0.0, prevAngle = 0.0, elapsedTimeSec=0.0;
unsigned long prevTimeStamp = 0, currTimeStamp = 0, elapsedTime = 0;

boolean isFirstTime = true;

float angleBuffer[ANGLE_BUFFER_LENGTH];
int angleBufferIndex = 0;

boolean MotorOff = false;

float curErr = 0, prevErr = 0, SumErr = 0;
float integralTerm = 0, derivativeTerm = 0;
float Cn = 0;

//float Kp = 0.0, Ki = 0.0,  Kd = 0.0;  //hard code pid numbers instead of pot


//MOTOR SETUP
const int bhi = 8;    //on 5v
const int ahi = 7;    //on 5v
const int ali = 6;    //reverse pwm
const int bli = 5;    //forward pwm
const int dis = 4;    //disable 5v=yes=coast

enum WheelDirection {
    DIR_FORWARD,
    DIR_BACKWARD,
    DIR_STOP
};

// CALIBRATE GYRO hold still for 3 seconds on startup. gyro will average 25 readings

float CalGyro() {

    float val = 0.0;

    delay(500);

    for (int i = 0; i < 25; i++) {

        val += analogRead(gyrox);

        delay(10);

      }

    val /= 25.0;

    
    return val;

}

float GetAvgAngle() {  //angle buffer used as a low pass filter
                       //increase anglebufferlength to average over more readings 
    int count = 0;    //set anglebuffer to 1 for no averaging

    float a = 0;

 

    for (int i = 0; i < ANGLE_BUFFER_LENGTH; i++) {

        if (angleBuffer[i] != 0) {

            count++;

            a += angleBuffer[i];

        }

    }
    
    if (count > 0)

        return a / float(count);

    else
    
        return 0.0;

}

float GetAccAngle() {

    float r = sqrt((float) gacclz2 * (float) gacclz2 + (float) gaccly2 * (float) gaccly2);

    accAngle = (float) gacclz2 / r; //approximates sine
    
 
    return accAngle;

}

float EstimateAngle() {

    currTimeStamp = micros();

    elapsedTime = currTimeStamp - prevTimeStamp;

    prevTimeStamp = currTimeStamp;

 

    elapsedTimeSec = (float) elapsedTime / 1e6f;

 

    float angle = GetAccAngle();

    if (isFirstTime) {

        currAngle = angle;

        isFirstTime = false;

    } else {

        VxGyro = analogRead(gyrox);

        degX = (float) (VxGyro - GYRO_X_INIT_VAL) * 3300.0f / 512.0f / 180.0f * (float) elapsedTimeSec;
        degX = degX * - 1.0f;    // Invert Gyro Reading
        currAngle = 0.995 * (prevAngle + degX) + 0.005 * angle;

    }

    prevAngle = currAngle;

 
    if (MotorOff) { //for debugging purposes

        Serial.print("\tcurrAngleAvg=");

        Serial.print(GetAvgAngle(), 4);

        Serial.print("\tcurrAngle=");

        Serial.print(currAngle, 4);

        Serial.print("\taccAngle=");

        Serial.print(accAngle);

        Serial.print("\tgyroAngle=");

        Serial.print(degX + prevAngle);    //* 2864.788975 = degrees

        Serial.print("\tVxGyro=");

        Serial.print(VxGyro);

        Serial.print("\telapsedTime=");

        Serial.print(elapsedTime);

        Serial.print("\tthrottle=");

        Serial.println(throttle);

    }

    return currAngle;
    }

// MOTOR DRIVE
void MoveWheels(WheelDirection (dir), int speed) {

    if (MotorOff) return;

    if (speed > 255) speed = 255;

    else if (speed < 0) speed = 0;

    if (dir == DIR_STOP) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        digitalWrite(bli, LOW);

    } else if (dir == DIR_FORWARD) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        analogWrite(bli, speed);


    } else if (dir == DIR_BACKWARD) {

        digitalWrite(ahi, HIGH);      //set ahi and bhi osmc 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        analogWrite(ali, speed);
        digitalWrite(bli, LOW);
    }

}


void setup()
{
  Serial.begin(19200);

  pinMode(accly, INPUT);
  pinMode(acclz, INPUT);
  pinMode(gyrox, INPUT);
  
  analogReference (EXTERNAL);    //gyro and acl use 3.3vref, connect 3.3v to vref
 
 GYRO_X_INIT_VAL = CalGyro();    //gyro calibrates once on startup hold still for 3 seconds
 
        pinMode(ahi, OUTPUT);        //enable motor control pins
        pinMode(bhi, OUTPUT);
        pinMode(dis, OUTPUT);
        pinMode(ali, OUTPUT);
        pinMode(bli, OUTPUT);
        
        digitalWrite(ahi, HIGH);      //set motor to stop 
        digitalWrite(bhi, HIGH);
        digitalWrite(dis, LOW);
        digitalWrite(ali, LOW);
        digitalWrite(bli, LOW);
  
 
}

void loop()
{

// READ ACCELEROMETERS AND GYRO  

// Y ACCELEROMETER
voltsaccly = analogRead (1);
gaccly = voltsaccly * 3.3 / 1024;  
gaccly2 = (gaccly - 1.368) / .3;

// Z ACCELEROMETER
voltsacclz = analogRead (2);
gacclz = voltsacclz * 3.3 / 1024;  
gacclz2 = (gacclz - 1.460) / .3;

// read the tuning knob value:
  sensorValue1 = analogRead (analogInPin1);
  sensorValue2 = analogRead (analogInPin2);
  sensorValue3 = analogRead (analogInPin3);
 
  
  // map tuning knob to the range of the analog out:
  outputValue1 = map(sensorValue1, 0, 1023, 0, 2000);    //P Knob
  outputValue2 = map(sensorValue2, 0, 1023, 0, 20000);    //I Knob
  outputValue3 = map(sensorValue3, 0, 1023, 0, 1000);    //D Knob
  

//PID CALCULATION
    
    //Get Estimated Angle
    float a = EstimateAngle() - INIT_ANGLE;

 
    //Buffer the angle get an average
    angleBuffer[angleBufferIndex] = a;

    angleBufferIndex = (angleBufferIndex + 1) % ANGLE_BUFFER_LENGTH;

    float ang = GetAvgAngle();

 
    //CURRENT ERROR
    curErr = ang - INIT_ANGLE; //error   

    SumErr += curErr;

    if (SumErr > SumErrMax) SumErr = SumErrMax;

    else if (SumErr < SumErrMin) SumErr = SumErrMin;

    // Ki*SumE/(Kp*Fs*X)
    
    // INTEGRAL TERM
    integralTerm = SumErr * elapsedTimeSec * float(outputValue2) / float(outputValue1);
    
    // DERIVATIVE TERM
    derivativeTerm = curErr - prevErr;

    // Kd(curErr-prevErr)*Ts/(Kp*X)
    
    derivativeTerm = derivativeTerm * 10000 * float(outputValue3) * elapsedTimeSec / float(outputValue1);

    //PID CALC
    Cn = (curErr + integralTerm + derivativeTerm) * float(outputValue1);

    //MOTOR DRIVE

    WheelDirection dir;

    if (Cn > 0) dir = DIR_FORWARD;

    else if (Cn < -0) dir = DIR_BACKWARD;

    else dir = DIR_STOP;

    throttle = abs(Cn);

    if (abs(ang) > 0.7)  MoveWheels(DIR_STOP, 0); //if angle too large to correct, stop motor       

    else MoveWheels(dir, throttle);
    
    prevErr = curErr;
    
}