main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "MPU6050.h"
00004 #include "ledControl.h"
00005 #include "Motor.h"
00006 #include "HALLFX_ENCODER.h"
00007 
00008 Serial pc(USBTX, USBRX);       // Create terminal link
00009 Serial blue(p28, p27);          // Bluetooth TX, RX
00010 
00011 MPU6050 mpu6050;               // mpu6050 object from MPU6050 classs
00012 
00013 Ticker toggler1;               // Ticker for led toggling
00014 Ticker filter;                 // Ticker for periodic call to compFilter funcçs 
00015 Ticker balance;                 // Periodic routine for PID control of balance system
00016 Ticker speed;                  // Periodic routine for speed control
00017 Ticker bluetooth;              // Ticker for navigation
00018 
00019 Motor A(p23, p5, p6);       // pwm, fwd, rev
00020 Motor B(p24, p7, p8);       // pwm, fwd, rev
00021 
00022 /* Encoders*/
00023 HALLFX_ENCODER leftEncoder(p13);
00024 HALLFX_ENCODER rightEncoder(p14);
00025 
00026 HCSR04  usensor(p25,p26);   //trig pin,echo pin
00027 
00028 /* Function prototypes */
00029 void toggle_led1();
00030 void toggle_led2();
00031 void compFilter();
00032 void balancePID();
00033 void balanceControl();
00034 void Forward(float);
00035 void Reverse(float);
00036 void Stop(void);
00037 void Navigate();
00038 void TurnRight(float);
00039 void TurnLeft(float);
00040 
00041 /* Variable declarations */
00042 float pitchAngle = 0;
00043 float rollAngle = 0;
00044 bool dir;           // direction of movement
00045 float Kp = 0.5;
00046 float Ki = 0.00001;
00047 float Kd = 0.01;//0.01;//0.05;
00048 float set_point = 1.005;         // Angle for staying upright
00049 float errorPID = 0;             // Proportional term (P) in PID
00050 float last_errorPID =0;         // Previous error value
00051 float integral = 0;             // Integral (I)
00052 float derivative = 0;           // Derivative (D)
00053 float outputPID = 0;            // Output of PID calculations
00054 float position = 0.0;
00055 float dist=0.0;
00056 float range = 0.000;
00057 int phone_char;
00058 DigitalOut myled(LED4);
00059 
00060 int main()
00061 {
00062     pc.baud(9600);                               // baud rate: 9600
00063     blue.baud(9600);
00064     
00065     mpu6050.whoAmI();                            // Communication test: WHO_AM_I register reading 
00066     wait(0.5);
00067     mpu6050.calibrate(accelBias,gyroBias);       // Calibrate MPU6050 and load biases into bias registers
00068     pc.printf("Calibration is completed. \r\n"); 
00069     mpu6050.init();                              // Initialize the sensor
00070     pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
00071    
00072     filter.attach(&compFilter, 0.005);         // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)  
00073     balance.attach(&balancePID,  0.010);       // Same period with balancePID
00074     speed.attach(&balanceControl, 0.01);
00075     bluetooth.attach(&Navigate, 0.05);
00076      
00077     while(1)
00078     {  
00079      
00080     if(blue.readable()) 
00081         {       
00082         phone_char = blue.getc();
00083         pc.putc(phone_char);
00084         pc.printf("Bluetooth Start\r\n");
00085         myled = !myled;
00086         }
00087         
00088         pc.printf("Roll Angle: %.1f, Pitch Angle: %.1f\r\n",rollAngle,pitchAngle);
00089         pc.printf("Error = %f\r\n",outputPID);
00090         wait_ms(40);
00091     }    
00092 }
00093 
00094 void toggle_led1() {ledToggle(1);}
00095 void toggle_led2() {ledToggle(2);}
00096 
00097 /* This function is calls the complementary filter with pitch and roll angles */ 
00098 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}
00099 
00100 void balancePID()
00101 {
00102     errorPID = set_point - pitchAngle;          //Proportional (P) 
00103     integral += errorPID;                       //Integral (I)
00104     derivative = errorPID - last_errorPID;      //Derivative (D)
00105     
00106     last_errorPID = errorPID;                   //Save current value for next iteration
00107     
00108     outputPID = (Kp * errorPID) + (Ki * integral) + (Kd * derivative);  //PID calculation
00109     
00110     /* errorPID is restricted between -1 and 1 */ 
00111     if(outputPID > 0.8)
00112         outputPID = 0.8;
00113     else if(outputPID < -0.8)
00114         outputPID = -0.8;   
00115 }
00116 
00117 void balanceControl() 
00118 {    
00119     int direction=0;                                            // Variable to hold direction we want to drive
00120     if (outputPID>0)direction=1;                                 // Positive speed indicates forward
00121     if (outputPID<0)direction=2;                                 // Negative speed indicates backwards
00122     if((abs(pitchAngle)>10.00))direction=0; 
00123   
00124     switch( direction) {                                        // Depending on what direction was passed
00125         case 0:                                                 // Stop case
00126             Stop();
00127             break;
00128         case 1:                                                 // Forward case
00129             Forward(abs(outputPID));
00130             break;
00131         case 2:                                                 // Backwards
00132             Reverse(abs(outputPID));
00133             break;
00134         default:                                                // Catch-all (Stop)
00135             Stop();
00136             break;
00137     }    
00138 }
00139 
00140 void Stop(void)
00141 {
00142     A.speed(0.0);
00143     B.speed(0.0); 
00144 }
00145  
00146 void Forward(float fwd_dist)
00147  {
00148     A.speed(-fwd_dist);
00149     B.speed(-fwd_dist); 
00150  }
00151 
00152 void TurnRight(float ryt_dist)
00153  {
00154      A.speed(ryt_dist);
00155      B.speed(-ryt_dist);
00156  }
00157  
00158 void TurnLeft(float lft_dist)
00159  {
00160      A.speed(-lft_dist);
00161      B.speed(lft_dist);
00162  }
00163 
00164 void Reverse(float rev_dist)
00165  {
00166     A.speed(rev_dist);
00167     B.speed(rev_dist); 
00168  }
00169  
00170  void Navigate()
00171  {
00172      switch (phone_char)
00173             {
00174                 case 'f':
00175                 Forward(0.7);
00176                 break;
00177                 
00178                 case 'F':
00179                 Forward(0.7);
00180                 break;
00181                 
00182                 case 'b': 
00183                 Reverse(0.7);
00184                 break;
00185                 
00186                 case 'B': 
00187                 Reverse(0.7);
00188                 break;
00189                 
00190                 case 'r':
00191                 TurnRight(0.7);
00192                 break;
00193                 
00194                 case 'R':
00195                 TurnRight(0.7);
00196                 break;
00197                 
00198                 case 'l':
00199                 TurnLeft(0.7);
00200                 break;
00201                 
00202                 case 'L':
00203                 TurnLeft(0.7);
00204                 break;
00205                 
00206                 default:
00207                 Stop();
00208             }
00209  }
00210