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 // 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; }