BalancingRobot.cpp

00001 /*
00002  * The code is released under the GNU General Public License.
00003  * This is the algorithm for my balancing robot/segway.
00004  * It is controlled by Bluetooth
00005  */
00006 
00007 #include "mbed.h"
00008 #include "BalancingRobot.h"
00009 #include "MPU6050.h"
00010 #include "ledControl.h"
00011 #include "ultrasonic.h"
00012 
00013 //#include "HALLFX_ENCODER.h"
00014 
00015 /* Setup serial communication */
00016 Serial blue(PTE22, PTE23); // For remote control ps3
00017 Serial pc(USBTX, USBRX); // USB
00018 
00019 /* IMU */
00020 MPU6050 mpu6050;
00021 
00022 
00023 
00024 /* HC-SR04 */
00025 void dist(int distance)
00026 {
00027     //put code here to happen when the distance is changed
00028     
00029 }
00030 
00031 ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist);
00032 ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist);
00033 
00034 /* Setup timer instance */
00035 Timer t;
00036 
00037 /* Ticker for led toggling */
00038 Ticker toggler1;      
00039  
00040 
00041          
00042 int main() {
00043         
00044 
00045     
00046     
00047     /* Set baudrate */
00048     blue.baud(115200);
00049     pc.baud(115200);
00050       
00051     
00052     /* Set PWM frequency */
00053     leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
00054     rightPWM.period(0.00005);
00055     
00056     
00057     
00058     /* Calibrate the gyro and accelerometer relative to ground */
00059     mpu6050.whoAmI();                            // Communication test: WHO_AM_I register reading 
00060     wait(0.5);
00061     mpu6050.calibrate(accelBias,gyroBias);       // Calibrate MPU6050 and load biases into bias registers
00062     //pc.printf("Calibration is completed. \r\n"); 
00063     mpu6050.init();                              // Initialize the sensor
00064     //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
00065     
00066     
00067     
00068     /* Setup timing */
00069     t.start();
00070     
00071     loopStartTime = t.read_us();
00072     timer = loopStartTime;
00073     DesiredAngle = -2.5;
00074     ConstantAngle = DesiredAngle;
00075 
00076     while (1) {
00077         
00078         /* Calculate pitch angle using a Complementary filter */
00079         mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
00080         wait_us(5);
00081         timer = t.read_us();
00082         
00083         /* Drive motors */
00084         if (abs(pitchAngle)> 20) // Stop if falling or laying down
00085             stopAndReset();
00086         else
00087             PID(DesiredAngle, targetOffset);
00088             
00089         /* Update wheel velocity every 100ms 
00090         loopCounter++;
00091         if (loopCounter%10 == 0) 
00092         { // If remainder is equal 0, it must be 10,20,30 etc.
00093             LeftWheelPosition = leftEncoder.read();
00094             RightWheelPosition = rightEncoder.read();
00095             wheelPosition = LeftWheelPosition + RightWheelPosition;
00096             wheelVelocity = wheelPosition - lastWheelPosition;
00097             lastWheelPosition = wheelPosition;
00098             
00099             if (abs(wheelVelocity) <= 20 && !stopped) 
00100             { // Set new targetPosition if braking
00101                 targetPosition = wheelPosition;
00102                 stopped = true;
00103             }
00104         }
00105         */
00106         
00107         //pc.printf("Pitch: %f, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue);
00108                 
00109         //Check for incoming serial data 
00110         if (blue.readable()) // Check if there's any incoming data
00111             receiveBluetooth();
00112         
00113                  
00114            
00115         frontUltra.startUpdates();
00116 
00117         distF = frontUltra.getCurrentDistance();
00118 
00119         backUltra.startUpdates();
00120        
00121         distB = backUltra.getCurrentDistance(); 
00122         
00123         
00124         pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB);             
00125            
00126         /* Use a time fixed loop */
00127         lastLoopUsefulTime = t.read_us() - loopStartTime;
00128         if (lastLoopUsefulTime < STD_LOOP_TIME)
00129             wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
00130                         
00131         lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
00132         //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME);
00133         //pc.printf("anlik: %d\n\r",t.read_us());
00134         //pc.printf("lastLoopTime: %d\n\r",lastLoopTime);
00135         //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime);
00136         loopStartTime = t.read_us();
00137     }
00138 }
00139 
00140 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
00141 
00142 void toggle_led1() {ledToggle(1);}
00143 void toggle_led2() {ledToggle(2);}
00144 
00145 void PID(double restAngle, double offset) 
00146 {
00147     
00148     /* Steer robot */
00149     /*if (steerForward) 
00150     {
00151         /* offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
00152         restAngle -= offset; 
00153         restAngle -= -1.0;
00154         
00155         
00156     }
00157     else if (steerBackward) 
00158     {
00159         //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
00160         //restAngle += offset;
00161         restAngle += 1.0;
00162     }*/
00163     /* Brake */
00164     /*else if (steerStop)
00165     
00166         restAngle = 0.0; 
00167     {
00168         //long positionError = wheelPosition - targetPosition;
00169         
00170     /*  if (abs(positionError) > zoneA) // Inside zone A
00171             restAngle -= (double)positionError/positionScaleA;
00172         else if (abs(positionError) > zoneB) // Inside zone B
00173             restAngle -= (double)positionError/positionScaleB;
00174         else // Inside zone C
00175             restAngle -= (double)positionError/positionScaleC; */
00176         
00177         //restAngle -= (double)wheelVelocity/velocityScaleStop;
00178         
00179         /*if (restAngle < -5.00) // Limit rest Angle
00180             restAngle = -5.00;
00181         else if (restAngle > 5.00)
00182             restAngle = 5.00;
00183     }
00184     
00185     /* Update PID values */
00186     error1 = restAngle - pitchAngle;
00187     double pTerm = Kp * error1;
00188     iTerm += Ki * error1;
00189     double dTerm = Kd * (error1 - lastError);
00190     lastError = error1;
00191     
00192     PIDValue = pTerm + iTerm + dTerm;
00193     //pc.printf("DesiredAngle = %lf", DesiredAngle);
00194     //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd);
00195     
00196     /* Steer robot sideways */
00197     double PIDLeft;
00198     double PIDRight;   
00199     
00200     if (steerLeft) {
00201         PIDLeft = PIDValue-turnSpeed;
00202         PIDRight = PIDValue+turnSpeed;
00203     } else if (steerRotateLeft) {
00204         PIDLeft = PIDValue-rotateSpeed;
00205         PIDRight = PIDValue+rotateSpeed;
00206     } else if (steerRight) {
00207         PIDLeft = PIDValue+turnSpeed;
00208         PIDRight = PIDValue-turnSpeed;
00209     } else if (steerRotateRight) {
00210         PIDLeft = PIDValue+rotateSpeed;
00211         PIDRight = PIDValue-rotateSpeed;
00212     } else {
00213     
00214         if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) ))
00215             DesiredAngle -= rotateSpeed;
00216             
00217         else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) ))
00218         DesiredAngle += rotateSpeed;
00219         
00220         else {
00221         if(!steerForward && DesiredAngle < ConstantAngle)
00222             DesiredAngle += rotateSpeed;
00223             
00224         else if(!steerBackward && DesiredAngle > ConstantAngle)
00225             DesiredAngle -= rotateSpeed;
00226         }   
00227         
00228         PIDLeft = PIDValue;
00229         PIDRight = PIDValue;
00230     
00231     }
00232     //PIDLeft *= 0.95; // compensate for difference in the motors
00233     
00234     /* Set PWM Values */
00235     if (PIDLeft >= 0.0)
00236         move(left, backward, PIDLeft + 0.55);
00237     else
00238         move(left, forward, (PIDLeft - 0.55) * -1);
00239     if (PIDRight >= 0.0)
00240         move(right, backward, PIDRight + 0.55);
00241     else
00242         move(right, forward, (PIDRight - 0.55) * -1);
00243 //pc.printf("RestAngle3 %lf" ,restAngle);
00244 }
00245 
00246 void receiveBluetooth() {
00247     //char input[16]; // The serial buffer is only 16 characters
00248     char phone_char;
00249     //int i = 0;
00250     //while (1) {
00251         //input[i] = blue.getc();
00252         //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
00253             //break;
00254         //i++;
00255         
00256         //pc.printf("Im here..");
00257     //}
00258     phone_char = blue.getc();
00259     
00260     /* if(distB < 50 && distB > 4) {
00261         phone_char = 'S';
00262         if (distB < 20)
00263             phone_char = 'F';
00264     } */
00265     
00266     
00267     if(distB < 20 && distB > 4)
00268         phone_char = 'F';
00269     else if ( distB < 30)
00270         phone_char = 'S';
00271     else if (distB < 50)
00272         phone_char = 'B';
00273     
00274     
00275     pc.putc(phone_char);
00276     pc.printf("Input: %c\n",phone_char);
00277     
00278     // Set all false
00279     steerForward = false;
00280     steerBackward = false;
00281     steerStop = false;
00282     steerLeft = false;
00283     steerRotateLeft = false;
00284     steerRight = false;
00285     steerRotateRight = false;
00286     
00287     /* For remote control */
00288     if (/*input[0]*/phone_char == 'F') { // Forward
00289         //strtok(/*input*/phone_char, ","); // Ignore 'F'
00290         //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double
00291         //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
00292         //DesiredAnglee = 0.8;
00293         pc.printf("F");
00294         steerForward = true;
00295     } else if (/*input[0]*/phone_char == 'B') { // Backward
00296         //strtok(/*input*/phone_char, ","); // Ignore 'B'
00297         //targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
00298         //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
00299         //DesiredAnglee = -5.2;
00300         pc.printf("B");
00301         steerBackward = true;
00302     } else if (/*input[0]*/phone_char == 'L') { // Left
00303         if (/*input[0]*/phone_char == 'L') // Left Rotate
00304             steerRotateLeft = true;
00305         else
00306             steerLeft = true;
00307     } else if (/*input[0]*/phone_char == 'R') { // Right
00308         if (/*input[0]*/phone_char == 'R') // Right Rotate
00309             steerRotateRight = true;
00310         else
00311             steerRight = true;
00312     } else if (/*input[0]*/phone_char == 'S') { // Stop
00313         steerStop = true;
00314         stopped = false;
00315         //targetPosition = wheelPosition;
00316     } else if (/*input[0]*/phone_char == 'A') { // Abort
00317         stopAndReset();
00318         while (blue.getc() != 'C'); // Wait until continue is send
00319         
00320     }
00321 }
00322 
00323 void stopAndReset() {
00324     stop(both);
00325     lastError = 0;
00326     iTerm = 0;
00327     targetPosition = wheelPosition;
00328 }
00329 
00330 void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
00331     if (motor == left) {
00332         leftPWM = speed;
00333         if (direction == forward) {
00334             leftA = 0;
00335             leftB = 1;
00336         } else if (direction == backward) {
00337             leftA = 1;
00338             leftB = 0;
00339         }
00340     } else if (motor == right) {
00341         rightPWM = speed;
00342         if (direction == forward) {
00343             rightA = 0;
00344             rightB = 1;
00345         } else if (direction == backward) {
00346             rightA = 1;
00347             rightB = 0;
00348         }
00349     } else if (motor == both) {
00350         leftPWM = speed;
00351         rightPWM = speed;
00352         if (direction == forward) {
00353             leftA = 0;
00354             leftB = 1;
00355             
00356             rightA = 0;
00357             rightB = 1;
00358         } else if (direction == backward) {
00359             leftA = 1;
00360             leftB = 0;
00361             
00362             rightA = 1;
00363             rightB = 0;
00364         }
00365     }
00366 }
00367 void stop(Motor motor) {
00368     if (motor == left) {
00369         leftPWM = 1;
00370         leftA = 1;
00371         leftB = 1;
00372     } else if (motor == right) {
00373         rightPWM = 1;
00374         rightA = 1;
00375         rightB = 1;
00376     } else if (motor == both) {
00377         leftPWM = 1;
00378         leftA = 1;
00379         leftB = 1;
00380         
00381         rightPWM = 1;
00382         rightA = 1;
00383         rightB = 1;
00384     }
00385 }