PID_control.cpp

00001 
00002 #include "mbed.h"
00003 #include "QEI.h"
00004 
00005 QEI encoder_Right(PB_3, PA_15, NC, 360, QEI::X4_ENCODING);
00006 QEI encoder_Left(PA_1, PC_4, NC, 360, QEI::X4_ENCODING);
00007 double Kp = 0.27;//.005;
00008 double Ki = 0.001;//0.0000001;
00009 double Kd = 0.001;
00010 PwmOut m_Right_F(PB_10);
00011 PwmOut m_Right_B(PC_7);
00012 PwmOut m_Left_F(PA_7);
00013 PwmOut m_Left_B(PB_6);
00014 double i_speed = 0.15;
00015 double C_speed(0);
00016 int integrator = 0;
00017 int decayFactor = 1;
00018 double Error = 0;
00019 double prevError = 0;
00020 Serial pc (PA_2, PA_3); //serial comm enabled on pins pa_2 and pa_3
00021 Timer timer;
00022 int counter = 0;
00023 
00024 double P_controller(int error)
00025 {
00026     double correction = (Kp*error);
00027     return correction;
00028 }
00029 
00030 double I_controller(int error)
00031 {
00032     integrator += error;
00033     double correction = Ki*integrator;
00034     integrator /= decayFactor;
00035     return correction;
00036 }
00037 
00038 double D_controller(int error)
00039 {
00040     int dError = error - prevError;
00041     int dt = timer.read_us();
00042     timer.reset();
00043     prevError = error;
00044     double correction;
00045     if (dt==0)
00046         correction=0;
00047     else
00048         correction = Kd*dError/dt;
00049     return correction;
00050 }
00051 
00052 Ticker systicker;
00053 //speed = speed + P_Controller(error) + I_Controller(error) + D_Controller(error);
00054 void systick()
00055 {
00056     double R_en_count = encoder_Right.getPulses()/100;
00057     double L_en_count = encoder_Left.getPulses()/100;
00058     Error = R_en_count - L_en_count;
00059     double ex = D_controller(Error);
00060     // if (ex < 0)
00061     //    ex = -ex;
00062     C_speed = P_controller(Error) + I_controller(Error) + ex;
00063     if (C_speed < 0)
00064         C_speed = C_speed*-1;
00065 }
00066 
00067 
00068 void forward()
00069 {
00070     double f1_speed = i_speed + C_speed;
00071     double f2_speed = i_speed - C_speed;
00072 
00073     /*pc.printf("C: %f", C_speed);
00074     if (C_speed < 0)
00075         pc.printf("-");
00076     if (C_speed > 0)
00077         pc.printf("+");
00078     */
00079 
00080 
00081     if(f1_speed >= 0.7) {   //upper bound, can not go faster
00082         f1_speed = 0.7;
00083     }
00084     if (f1_speed <= 0.05)
00085         f1_speed = 0.05;
00086 
00087     if(f2_speed <= 0.05) {  //lower bound, should not be slower than this
00088         f2_speed = 0.05;
00089     }
00090 
00091     pc.printf(" f1: %f", f1_speed);
00092     pc.printf(" f2: %f", f2_speed);
00093 
00094     //problems when left wheel is held for the + case
00095     if (Error > 0) { //right wheel is turning more
00096         m_Left_F.write(f1_speed);
00097         m_Right_F.write(f2_speed); //f2_speed
00098     }
00099     if (Error < 0) { //left wheel is turning more
00100         m_Right_F.write(f1_speed);
00101         m_Left_F.write(f2_speed);  //f2_speed
00102     }
00103     if (Error == 0) {
00104         m_Right_F.write(i_speed);
00105         m_Left_F.write(i_speed);
00106     }
00107 }
00108 
00109 void backUp()
00110 {
00111     m_Left_F.write(0);
00112     m_Right_F.write(0);
00113     m_Left_B.write(i_speed);
00114     m_Right_B.write(i_speed);
00115     wait(.15);
00116 }
00117 
00118 void turnRight()
00119 {
00120     m_Left_B.write(0);
00121     m_Right_F.write(0);
00122     m_Left_F.write(i_speed);
00123     m_Right_B.write(i_speed);
00124     wait(.2);
00125 }
00126 
00127 void turnLeft()
00128 {
00129     m_Left_F.write(0);
00130     m_Right_B.write(0);
00131     m_Right_F.write(i_speed);
00132     m_Left_B.write(i_speed);
00133     wait(.2);
00134 }
00135 
00136 void turnAround()
00137 {
00138     m_Left_B.write(0);
00139     m_Right_F.write(0);
00140     m_Left_F.write(i_speed);
00141     m_Right_B.write(i_speed);
00142     wait(.4);
00143 }
00144 
00145 void stop()
00146 {
00147     m_Right_F.write(0);
00148     m_Right_B.write(0);
00149     m_Left_F.write(0);
00150     m_Left_B.write(0);    
00151 }
00152 
00153 void debugEncoder()
00154 {
00155     while(1) {
00156         wait(1);
00157         pc.printf("Right: %i", encoder_Right.getPulses());
00158         pc.printf("  Left: %i", encoder_Left.getPulses(), "\n");
00159         pc.printf("\n");
00160     }
00161 }
00162 
00163 void debugError()
00164 {
00165     while(1) {
00166         pc.printf("Error: %i\n", Error);
00167     }
00168 }
00169 
00170 AnalogIn RS_IRR(PA_0);
00171 AnalogIn RF_IRR(PA_4); //Right Front
00172 AnalogIn LF_IRR(PC_1); //Left Front
00173 AnalogIn LS_IRR(PC_0); //Left Side
00174  
00175 DigitalOut RS_IRE(PC_10);
00176 DigitalOut RF_IRE(PC_11); //Right Front
00177 DigitalOut LF_IRE(PB_0); //Left Front
00178 DigitalOut LS_IRE(PB_7); //Left Side 
00179 
00180  
00181 
00182 int main()
00183 {
00184     float threshold = 0.001;
00185     float turnThreshold = 0.001;
00186     printf("\nAnalogIn example\n");
00187     LF_IRE.write(1);
00188     RF_IRE.write(1);  
00189     while (1){ 
00190     while (LF_IRR.read() < threshold && RF_IRR.read() < threshold){
00191         forward();
00192         float value1 = LF_IRR.read();
00193         float value2 = RF_IRR.read(); 
00194         printf("LF Led: %f\n", value1);
00195         wait(0.5);
00196         printf("RF Led: %f\n", value2);
00197     }
00198     
00199     backUp();
00200     
00201     LS_IRE.write(1);
00202     RS_IRE.write(1);  
00203     if (LS_IRR.read() > turnThreshold)
00204         if (RS_IRR.read() < turnThreshold)
00205             turnRight();
00206         else
00207             turnAround();
00208     else if (RS_IRR.read() > turnThreshold)
00209         if (LS_IRR.read() < turnThreshold)
00210             turnRight();
00211         else
00212             turnAround();
00213     else
00214         turnAround();
00215     LS_IRE.write(0);
00216     RS_IRE.write(0); 
00217 }
00218     stop();
00219 }
00220     /*while(RF_IRR.read() * 100000 < 175 && LF_IRR.read() * 100000 < 175) {
00221         
00222         /*meas = LS_IRR.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
00223         meas = meas * 100000; // Change the value to be in the 0 to 3300 range
00224         printf("measure = %.0f mV\n", meas);
00225         */
00226         /*if (meas > 2000) { // If the value is greater than 2V then switch the LED on
00227           LS_IRE = 1;
00228         }
00229         else {
00230           LS_IRE = 0;
00231         }
00232         */
00233         /*
00234         forward();
00235         //wait(0.2); // 200 ms
00236     }
00237     stop();
00238     */
00239     
00240 
00241 
00242 
00243 /*int main()      //only runs once
00244 {
00245     systicker.attach_us(&systick, 1000);    //enable interrupt
00246     while (1) {
00247         forward();
00248     }
00249     //
00250     //debugEncoder();
00251 }
00252 */