PID_control.cpp

00001  //added routines to main for easier testing of movement functions
00002 //wrote testStop();
00003 //Kevin Lee Nov 30, 2017
00004 //edited testStop();
00005 //started a testTurns();
00006 //testTurns not working as thought
00007 //Myles Byrne Nov 30, 2017
00008 
00009 //TODO: test quarterTurnLeft(); , quarterTurnRight(); for delay constants 
00010 //TODO: finetune PID , Multiplexing for IR LEDS , 
00011 //SUGGESTIONS: implement turn functions using encoders. i.e. each pulse represents 1 degree, need difference of 90 pulses for right turn
00012 
00013 
00014 #include "mbed.h"
00015 #include "QEI.h"
00016 
00017 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
00018 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
00019 double Kp = 0.27;//.005;
00020 double Ki = 0.001;//0.0000001;
00021 double Kd = 0.001;
00022 PwmOut m_Right_F(PB_10);
00023 PwmOut m_Right_B(PC_7);
00024 PwmOut m_Left_F(PA_7);
00025 PwmOut m_Left_B(PB_6);
00026 double i_speed = 0.15;
00027 double C_speed = 0;
00028 int integrator = 0;
00029 int decayFactor = 1;
00030 double Error = 0;
00031 double IRError = 0; //added
00032 double prevError = 0;
00033 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
00034 Timer timer;
00035 int counter = 0;
00036 float threshold = 0.00085;
00037 float turnThreshold = 0.0004;
00038     
00039 
00040 
00041 //IRR = IR Reciver
00042 AnalogIn RS_IRR(PA_0); //Right Side 
00043 AnalogIn RF_IRR(PA_4); //Right Front
00044 AnalogIn LF_IRR(PC_1); //Left Front
00045 AnalogIn LS_IRR(PC_0); //Left Side
00046 
00047 //IRE = IR Emitter
00048 DigitalOut RS_IRE(PC_10); //Right Side
00049 DigitalOut RF_IRE(PC_11); //Right Front
00050 DigitalOut LF_IRE(PB_0); //Left Front
00051 DigitalOut LS_IRE(PB_7); //Left Side 
00052 
00053 double P_controller(int error)
00054 {
00055     double correction = (Kp*error);
00056     return correction;
00057 }
00058 
00059 double I_controller(int error)
00060 {
00061     integrator += error;
00062     double correction = Ki*integrator;
00063     integrator /= decayFactor;
00064     return correction;
00065 }
00066 
00067 double D_controller(int error)
00068 {
00069     int dError = error - prevError;
00070     int dt = timer.read_us();
00071     timer.reset();
00072     prevError = error;
00073     double correction;
00074     if (dt==0)
00075         correction=0;
00076     else
00077         correction = Kd*dError/dt;
00078     return correction;
00079 }
00080 
00081 Ticker systicker;
00082 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
00083 void systick()
00084 {
00085     double R_en_count = encoder_Right.getPulses()/100;
00086     double L_en_count = encoder_Left.getPulses()/100;
00087     Error = R_en_count - L_en_count;
00088     double ex = D_controller(Error);
00089     C_speed = P_controller(Error) + I_controller(Error) + ex;
00090     if (C_speed < 0)
00091         C_speed = C_speed*-1;
00092     float Front_IRError = RF_IRR.read() + LF_IRR.read();  //rev1
00093     float Side_IRError = RS_IRR.read() - LS_IRR.read();   //rev1,define side_error as positive if closer to right
00094 }
00095 
00096 
00097 void forward()
00098 {
00099     double f1_speed = i_speed + C_speed;
00100     double f2_speed = i_speed - C_speed;
00101 
00102     /*pc.printf("C: %f", C_speed);
00103     if (C_speed < 0)
00104         pc.printf("-");
00105     if (C_speed > 0)
00106         pc.printf("+");
00107     */
00108 
00109 
00110     if(f1_speed >= 0.7) {   //upper bound, can not go faster
00111         f1_speed = 0.7;
00112     }
00113     if (f1_speed <= 0.05)
00114         f1_speed = 0.05;
00115 
00116     if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
00117         f2_speed = 0.05;
00118     }
00119 
00120     pc.printf(" f1: %f", f1_speed);
00121     pc.printf(" f2: %f", f2_speed);
00122 
00123     //problems when left wheel is held for the + case
00124     if (Error > 0) { //right wheel is turning more
00125         m_Left_F.write(f1_speed);
00126         m_Right_F.write(f2_speed); //f2_speed
00127     }
00128     if (Error < 0) { //left wheel is turning more
00129         m_Right_F.write(f1_speed);
00130         m_Left_F.write(f2_speed);  //f2_speed
00131     }
00132     if (Error == 0) {
00133         m_Right_F.write(i_speed);
00134         m_Left_F.write(i_speed);
00135     }
00136 }
00137 
00138 void forwardSlow()
00139 {
00140     float iS_speed = 0.05;
00141     double f1_speed = iS_speed + C_speed;
00142     double f2_speed = iS_speed - C_speed;
00143 
00144     /*pc.printf("C: %f", C_speed);
00145     if (C_speed < 0)
00146         pc.printf("-");
00147     if (C_speed > 0)
00148         pc.printf("+");
00149     */
00150 
00151 
00152     if(f1_speed >= 0.15) {   //upper bound, can not go faster
00153         f1_speed = 0.15;
00154     }
00155     if (f1_speed <= 0.05)
00156         f1_speed = 0.05;
00157 
00158     if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
00159         f2_speed = 0.05;
00160     }
00161 
00162     pc.printf(" f1: %f", f1_speed);
00163     pc.printf(" f2: %f", f2_speed);
00164 
00165     //problems when left wheel is held for the + case
00166     if (Error > 0) { //right wheel is turning more
00167         m_Left_F.write(f1_speed);
00168         m_Right_F.write(f2_speed); //f2_speed
00169     }
00170     if (Error < 0) { //left wheel is turning more
00171         m_Right_F.write(f1_speed);
00172         m_Left_F.write(f2_speed);  //f2_speed
00173     }
00174     if (Error == 0) {
00175         m_Right_F.write(iS_speed);
00176         m_Left_F.write(iS_speed);
00177     }
00178 }
00179 
00180 void backUp()
00181 {
00182     m_Left_F.write(0);
00183     m_Right_F.write(0);
00184     m_Left_B.write(i_speed);
00185     m_Right_B.write(i_speed);
00186     wait(.15);
00187 }
00188 
00189 void turnRight()
00190 {
00191     m_Left_B.write(0);
00192     m_Right_F.write(0);
00193     m_Left_F.write(i_speed);
00194     m_Right_B.write(i_speed);
00195     wait(.2);
00196 }
00197 
00198 void quarterTurnRight() //rev1, for ladders
00199 {
00200     m_Left_B.write(0);
00201     m_Right_F.write(0);
00202     m_Left_F.write(i_speed);
00203     m_Right_B.write(i_speed);
00204     wait(.1); //delay needs testing
00205 }
00206 
00207 void turnLeft()
00208 {
00209     m_Left_F.write(0);
00210     m_Right_B.write(0);
00211     m_Right_F.write(i_speed);
00212     m_Left_B.write(i_speed);
00213     wait(.2); //this is dependent on i_speed, can we write a function that varies with i_speed?
00214 }
00215 
00216 void quarterTurnLeft() { //rev1, for ladders
00217     m_Left_F.write(0);
00218     m_Right_B.write(0);
00219     m_Right_F.write(i_speed);
00220     m_Left_B.write(i_speed);
00221     wait(.1); //time needs testing
00222 }
00223 
00224 void turnAround()
00225 {
00226     m_Left_B.write(0);
00227     m_Right_F.write(0);
00228     m_Left_F.write(i_speed);
00229     m_Right_B.write(i_speed);
00230     wait(.4);
00231 }
00232 
00233 void stop()
00234 {
00235     m_Right_F.write(0);
00236     m_Right_B.write(0);
00237     m_Left_F.write(0);
00238     m_Left_B.write(0);    
00239 }
00240 
00241 void debugEncoder()
00242 {
00243     while(1) {
00244         wait(1);
00245         pc.printf("Right: %i", encoder_Right.getPulses());
00246         pc.printf("  Left: %i", encoder_Left.getPulses(), "\n");
00247         pc.printf("\n");
00248     }
00249 }
00250 
00251 void debugError()
00252 {
00253     while(1) {
00254         pc.printf("Error: %i\n", Error);
00255     }
00256 }
00257 
00258 
00259 /*void main1() {
00260     printf("\nAnalogIn example\n");
00261     LF_IRE.write(1);
00262     RF_IRE.write(1);  
00263     while (1){ 
00264         while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
00265             forward();
00266             float value1 = LF_IRR.read();
00267             float value2 = RF_IRR.read(); 
00268             printf("LF Led: %f\n", value1);
00269             wait(0.5);
00270             printf("RF Led: %f\n", value2);
00271             }
00272             
00273     backUp();
00274     LS_IRE.write(1);
00275     RS_IRE.write(1);  
00276     
00277     if (LS_IRR.read() > turnThreshold) {
00278         if (RS_IRR.read() < turnThreshold)
00279             turnRight();
00280         else
00281             turnAround(); 
00282         }
00283         
00284     else if (RS_IRR.read() > turnThreshold) {
00285         if (LS_IRR.read() < turnThreshold)
00286             turnRight();
00287         else
00288             turnAround();
00289             }
00290     else
00291         turnAround();
00292         
00293     LS_IRE.write(0);
00294     RS_IRE.write(0); 
00295     }
00296     stop();
00297 } 
00298 */
00299 
00300 
00301 void testStop() //rev1
00302 {
00303     //printf("\nAnalogIn example\n");
00304     LF_IRE.write(1); //turn on Left Front IR LED
00305     RF_IRE.write(1); //turn on Right Front IR LED
00306     forward(); //move forward slowly
00307     while(1) { //loop
00308         
00309         if (RF_IRR.read() > threshold && LF_IRR.read() > threshold) //IR Receivers will read higher analog values than Threshold if wall is near
00310         {
00311             stop(); //stop the rat
00312             forwardSlow();
00313             if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
00314             {
00315                 stop();
00316                 break;
00317             }
00318             
00319         }
00320         float value1 = LF_IRR.read();
00321         float value2 = RF_IRR.read(); 
00322         printf("LF Led: %f\n", value1); //output the read values from IR Receiver
00323         wait(0.25);
00324         printf("RF Led: %f\n\r", value2);
00325     }
00326             
00327 }
00328 
00329 
00330 void testTurnAround() //rev1
00331 {
00332     
00333 }
00334 
00335 void testTurns()
00336 {
00337     LF_IRE.write(1);
00338     RF_IRE.write(1);
00339     forward();  
00340     while(1)
00341     {
00342         if (RF_IRR.read() > turnThreshold && LF_IRR.read() > turnThreshold)
00343         {
00344             stop();
00345             forwardSlow();
00346             if (RF_IRR.read() > threshold && LF_IRR.read() > threshold)
00347             {
00348                 stop();
00349                 if (LS_IRR.read() > turnThreshold) {
00350                     if (RS_IRR.read() < turnThreshold)
00351                         turnRight();
00352                     else
00353                         turnAround(); 
00354                 }
00355         
00356                 else if (RS_IRR.read() > turnThreshold) {
00357                     if (LS_IRR.read() < turnThreshold)
00358                         turnLeft();
00359                     else
00360                         turnAround();
00361                 }
00362                 else
00363                     turnAround();
00364         
00365                 LS_IRE.write(0);
00366                 RS_IRE.write(0); 
00367             }
00368             //stop();
00369             break;
00370         }
00371     }
00372         float value1 = LF_IRR.read();
00373         float value2 = RF_IRR.read(); 
00374         printf("LF Led: %f\n", value1);
00375         wait(0.25);
00376         printf("RF Led: %f\n\r", value2);
00377 }
00378 
00379 
00380 
00381 int main()
00382 {
00383     systicker.attach_us(&systick, 1000);    //rev1
00384     testStop();
00385 }
00386 
00387 
00388 
00389 
00390 
00391 
00392     /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
00393         
00394         meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00395         meas = meas * 100000; // Change the value to be in the 0 to 3300 range
00396         printf("measure = %.0f mV\n", meas);
00397         
00398         /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
00399           LS_IRE = 1;
00400         }
00401         else {
00402           LS_IRE = 0;
00403         }
00404         */
00405         /*
00406         forward();
00407         //wait(0.2); // 200 ms
00408     }
00409     stop();
00410     */
00411     
00412 
00413 
00414 
00415 /*int main()      //only runs once
00416 {
00417     systicker.attach_us(&systick, 1000);    //enable interrupt
00418     while (1) {
00419         forward();
00420     }
00421     //
00422     //debugEncoder();
00423 }
00424 */