main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "basics.h"
00004 Ticker Systicker;
00005 Serial pc(PA_11, PA_12); // setting up RX TX
00006 
00007 DigitalOut led_red(PB_12);
00008 DigitalOut led_blue(PB_13);
00009 
00010 AnalogIn   LMR(PC_5);
00011 DigitalOut LME(PC_7);
00012 AnalogIn   ULR(PC_4);
00013 DigitalOut ULE(PB_0);
00014 AnalogIn   URR(PC_1);
00015 DigitalOut URE(PB_1);
00016 AnalogIn   RMR(PC_0);
00017 DigitalOut RME(PC_6);
00018 
00019 PwmOut Motor_LF(PA_6);
00020 PwmOut Motor_LB(PA_7);
00021 PwmOut Motor_RF(PB_6);
00022 PwmOut Motor_RB(PB_7);
00023 
00024 QEI MLEncoder (PA_0, PA_1, NC, 12); //left encoder
00025 QEI MREncoder (PA_3, PA_2, NC, 12); //right encoder
00026 //general
00027 double speed = 0.09;
00028 double speed_right = speed * 0.860;
00029 double pulsePerInch = 700;
00030 bool PID_enable=false;
00031 
00032 //encoders
00033 double encode1, encode2;
00034 double ML_pulse, MR_pulse, MR_prev_pulse, ML_prev_pulse;
00035 double encoderDif, integral=0;
00036 double P_value = 0.000000001838;
00037 double I_value = 0.0000;
00038 
00039 //IRs
00040 double leftIR, rightIR, upperRIR, upperLIR;
00041 double kp_IR=0.000005;
00042 double IR_dif, IR_intgral;
00043 
00044 //Flags
00045 bool stuck_flag = 0;
00046 
00047 void systick() // PID
00048 {
00049     MR_prev_pulse = MR_pulse;
00050     ML_prev_pulse = ML_pulse;
00051     ML_pulse = abs(MLEncoder.getPulses());
00052     MR_pulse = (abs(MREncoder.getPulses())/1.125);
00053     //MR_pulse = abs(MREncoder.getPulses());
00054 
00055     if ((abs(ML_pulse - ML_prev_pulse) < 1000) &&(abs(MR_pulse - MR_prev_pulse ) < 1000)) {
00056         stuck_flag = 1;
00057     }
00058 
00059 
00060     if (!PID_enable) return;
00061     if (IR_dif > -0.20 && IR_dif < 0.20) {
00062         Motor_LF = speed;
00063         Motor_RF = speed_right;
00064     } else {
00065         Motor_LF = Motor_LF + kp_IR* IR_dif;
00066         Motor_RF = Motor_RF - kp_IR* IR_dif ;
00067     }
00068 
00069 
00070 }
00071 ////////////////////////////////////////////////////////////////////////////
00072 ///////////////////// STOP AND GO ////////////////////////////////////////////
00073 /////////////////////////////////////////////////////////////////////////////
00074 void proceed()
00075 {
00076     PID_enable = true;
00077     Motor_LF = speed;
00078     Motor_LB = 0;
00079     Motor_RF = speed_right;
00080     Motor_RB = 0;
00081 }
00082 void all_motor_off()
00083 {
00084     Motor_LF = 0;
00085     Motor_LB = 0;
00086     Motor_RF = 0;
00087     Motor_RB = 0;
00088 }
00089 void stop()
00090 {
00091     PID_enable = false;
00092     led_red = 1;
00093 
00094     Motor_LF =0;
00095     Motor_LB =speed;
00096     Motor_RF =0;
00097     Motor_RB =speed_right;
00098     wait (0.03);
00099     all_motor_off();
00100     wait (0.02);
00101 
00102 }
00103 void travelNCell(double N )
00104 {
00105     MLEncoder.reset();
00106     MREncoder.reset();
00107 
00108     while (MLEncoder.getPulses() < N*3000 && MREncoder.getPulses() < N*3000 ) {
00109         Motor_LF = speed;
00110         Motor_LB = 0;
00111         Motor_RF = speed_right;
00112         Motor_RB =0;
00113     }
00114 }
00115 
00116 ////////////////////////////////////////////////////////////////////////////
00117 /////////////////////TURNING ////////////////////////////////////////////
00118 /////////////////////////////////////////////////////////////////////////////
00119 
00120 void turnLeft()
00121 {
00122     PID_enable = false;
00123     MLEncoder.reset();
00124     MREncoder.reset();
00125 
00126     while (MREncoder.getPulses() < 520 ) {
00127         Motor_LF = 0;
00128         Motor_LB = speed;
00129         Motor_RF = speed_right;
00130         Motor_RB = 0;
00131     }
00132 
00133     all_motor_off();
00134     // wait (0.5);
00135     PID_enable = true;
00136 }
00137 void turnRight(double N = 1) // in case if want to turn 180, pass in N = 2.5
00138 {
00139     PID_enable = false;
00140     MLEncoder.reset();
00141     MREncoder.reset();
00142 
00143     while (MLEncoder.getPulses() < 650 * N ) {
00144         Motor_LF = speed;
00145         Motor_LB = 0;
00146         Motor_RF = 0;
00147         Motor_RB = speed;
00148 
00149     }
00150     all_motor_off();
00151     wait (0.5);
00152     PID_enable = true;
00153     proceed();
00154 }
00155 
00156 ////////////////////////////////////////////////////////////////////////////
00157 ///////////////////// SENSORS ////////////////////////////////////////////
00158 /////////////////////////////////////////////////////////////////////////////
00159 void getEncoderValue()
00160 {
00161     ML_pulse = abs(MLEncoder.getPulses());
00162     MR_pulse = (abs(MREncoder.getPulses())/1.125); // Right encoder is 1.125 time faster
00163     encoderDif = abs (ML_pulse - MR_pulse);
00164     // pc.printf ("\n%f\t%f", ML_pulse,MR_pulse);
00165     //pc.printf("\n%f",(ML_pulse - MR_pulse));
00166     //integral = integral + encoderDif;
00167 
00168 }
00169 
00170 void getSensorValue()
00171 {
00172     RME=1;
00173     rightIR=RMR.read();
00174     LME=1;
00175     leftIR=LMR.read();
00176     URE=1;
00177     upperRIR=URR.read();
00178     ULE=1;
00179     upperLIR=ULR.read();
00180     IR_dif = leftIR - rightIR -0.15;
00181    // pc.printf("\n\n%f\n%f\n%f\n%f\n%f", leftIR,upperLIR, upperRIR,rightIR,IR_dif );
00182 //    wait (0.5);
00183 }
00184 
00185 ////////////////////////////////////////////////////////////////////////////
00186 ///////////////////// SPEED AND WALL ////////////////////////////////////////////
00187 /////////////////////////////////////////////////////////////////////////////
00188 void checkSpeed()
00189 {
00190     if (Motor_LF >( speed+0.1) || Motor_RF > (speed_right + 0.10)) {
00191         Motor_LF = speed;
00192         Motor_RF = speed_right;
00193     } else if (Motor_LF >( speed-0.1) || Motor_RF > (speed_right -0.10)) {
00194         Motor_LF = speed;
00195         Motor_RF = speed_right;
00196     }
00197 }
00198 
00199 bool checkDeadEnd()
00200 {
00201     if (upperRIR >0.40 &&  upperLIR > 0.40 && rightIR > 0.17 && leftIR >0.17 ) {
00202         PID_enable = false;
00203 
00204         Motor_LF =0;
00205         Motor_LB =speed;
00206         Motor_RF =0;
00207         Motor_RB =speed_right;
00208         wait (0.1);
00209         all_motor_off();
00210         wait (0.02);
00211         turnRight(2.5);  // truns out it takes more to do 180
00212         PID_enable = true;
00213         return true;
00214     }
00215     return false;
00216 
00217 }
00218 
00219 bool ifFrontWall()
00220 {
00221     if (upperRIR >0.40 &&  upperLIR > 0.40) {
00222         PID_enable = false;
00223 
00224         Motor_LF =0;
00225         Motor_LB =speed;
00226         Motor_RF =0;
00227         Motor_RB =speed;
00228         wait (0.03);
00229         all_motor_off();
00230         wait (0.02);
00231         PID_enable = true;
00232         return true;
00233     }
00234     return false;
00235 }
00236 
00237 
00238 
00239 void goThroughMaze()
00240 {
00241     if (ifFrontWall()) {
00242         if (rightIR < 0.1)
00243             turnRight();
00244         else if (leftIR < 0.1)
00245             turnLeft();
00246         else
00247             stop(); // if it goes to a deadend then stop
00248     }
00249 }
00250 
00251 
00252 void wallFollower()
00253 {
00254     if (rightIR < 0.05) {
00255         stop();
00256         wait(1);
00257         travelNCell(0.5);
00258         all_motor_off();
00259         wait (1);
00260         turnRight();
00261     }
00262     checkDeadEnd();
00263 }
00264 
00265 void checkIfStuck()
00266 {
00267     if ( ! stuck_flag ) return;
00268     if (MR_pulse < 1000 && ML_pulse < 1000) return;
00269 
00270     //MLEncoder.reset();
00271     //MREncoder.reset();
00272     PID_enable = 0;
00273     Motor_LF = 0;
00274     Motor_LB = speed;
00275     Motor_RF = 0;
00276     Motor_RB = speed_right;
00277     
00278     wait(3);
00279     //while (abs(MREncoder.getPulses()) < 520 && abs(MLEncoder.getPulses()) < 520 ) {
00280 //        Motor_LF = 0;
00281 //        Motor_LB = speed;
00282 //        Motor_RF = 0;
00283 //        Motor_RB = speed_right;
00284 //    }
00285 
00286     all_motor_off();
00287     wait(5);
00288     proceed();
00289     stuck_flag =0;
00290     PID_enable = 1;
00291 }
00292 
00293 
00294 ////////////////////////////////////////////////////////////////////////////
00295 ///////////////////// MAIN ////////////////////////////////////////////
00296 /////////////////////////////////////////////////////////////////////////////
00297 
00298 int main()
00299 {
00300     Motor_LF = speed;
00301     Motor_LB = 0;
00302     Motor_RF = speed_right;
00303     Motor_RB = 0;
00304     MLEncoder.reset();
00305     MREncoder.reset();
00306     Systicker.attach_us(&systick, 100);
00307     led_red = 0;
00308     while (1) {
00309         //pc.printf ("\n\nEncoder Pulses:\n""Encoder left:%d Encoder right: %d", MLEncoder.getPulses(), MREncoder.getPulses());
00310         PID_enable = 1;
00311         //getEncoderValue();
00312         getSensorValue();
00313 
00314         wallFollower();
00315 
00316 
00317         //checkIfStuck();
00318 
00319         //while(1){
00320 //            PID_enable = 1;
00321 //            all_motor_off();
00322 //            travelNCell(1);
00323 //            all_motor_off();
00324 //            wait(1);
00325 //
00326 //            }
00327 
00328 
00329 
00330 
00331     }
00332 }