main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "SongPlayer.h"
00004  
00005 RawSerial  pc(USBTX, USBRX);
00006 RawSerial  dev(p28,p27);
00007 DigitalOut led1(LED1);
00008 DigitalOut led4(LED4);
00009 
00010 // Set up the motor pins
00011 PwmOut motorACtrl(p21);
00012 PwmOut motorBCtrl(p22);
00013 DigitalOut backA(p20);
00014 DigitalOut fwdA(p19);
00015 DigitalOut stby(p18);
00016 DigitalOut fwdB(p17);
00017 DigitalOut backB(p16);
00018 Timer t;
00019 
00020 // Set up Hall-effect control
00021 DigitalIn EncR(p13); // right motor
00022 DigitalIn EncL(p14); // left motor
00023 int oldEncR = 0; // Previous encoder values
00024 int oldEncL = 0;
00025 int ticksR = 0; // Encoder wheel state change counts
00026 int ticksL = 0;
00027 int E; // Error  between the 2 wheel speeds
00028 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
00029 float speedR = 0; //Same for right wheel
00030 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
00031 
00032 // Distance sensor stuff
00033 AnalogIn irSensor(p15);         // Sensor
00034 bool emergencyStop = false;
00035 
00036 float timeout = 0.05f;
00037 int  count,ended;
00038 char buf[256];
00039 char motorCmd[] = "stop";
00040  
00041 /**
00042  *  Get the reply string from the WiFi module
00043  */
00044 void getreply()
00045 {
00046     memset(buf, '\0', sizeof(buf));
00047     t.start();
00048     ended=0;
00049     count=0;
00050     while(!ended) {
00051         if(dev.readable()) {
00052             buf[count] = dev.getc();
00053             count++;
00054         }
00055         if(t.read() > timeout) {
00056             ended = 1;
00057             t.stop();
00058             t.reset();
00059         }
00060     }
00061 }
00062 
00063 /**
00064  *
00065  */
00066 void getMotorCommand() {
00067     int index = 3;
00068     // iterate backwards through the message buffer
00069     for (int i = sizeof(buf) - 1; i > 2; i--){
00070         // Check if the current character is a null
00071         if (buf[i] != '\0') {
00072             motorCmd[index] = buf[i - 2];   // Copy the character
00073             if (index == 0){
00074                 break;      // Break if we have completed getting the command
00075             } else {
00076                 index--;    // Decrement otherwise
00077             }
00078         }
00079     }
00080 }
00081 
00082 /**
00083  *  Run the motor based on the command passed through the serial interface.
00084  */
00085 void runMotor(){
00086     if (strcmp(motorCmd, (char *)"fwrd") == 0) {
00087            stby = 1;        // Start H-bridge driver
00088            fwdA = 1;
00089            fwdB = 1;
00090            backA = 0;
00091            backB = 0;
00092            motorACtrl = speedL;
00093            motorBCtrl = speedR;
00094     } else if (strcmp(motorCmd, (char *)"back") == 0) {
00095            stby = 1;        // Start H-bridge driver
00096            fwdA = 0;
00097            fwdB = 0;
00098            backA = 1;
00099            backB = 1;
00100            motorACtrl = speedL;
00101            motorBCtrl = speedR;
00102     } else if (strcmp(motorCmd, (char *)"left") == 0) {
00103            stby = 1;        // Start H-bridge driver
00104            fwdA = 1;
00105            fwdB = 0;
00106            backA = 0;
00107            backB = 1;
00108            motorACtrl = speedL;
00109            motorBCtrl = speedR;
00110     } else if (strcmp(motorCmd, (char *)"rght") == 0) {
00111            stby = 1;        // Start H-bridge driver
00112            fwdA = 0;
00113            fwdB = 1;
00114            backA = 1;
00115            backB = 0;
00116            motorACtrl = speedL;
00117            motorBCtrl = speedR;
00118     } else if (strcmp(motorCmd, (char *)"stop") == 0) {
00119            stby = 1;        // Start H-bridge driver
00120            fwdA = 0;
00121            fwdB = 0;
00122            backA = 0;
00123            backB = 0;
00124            motorACtrl = 0.0f;   // Set speed to 0
00125            motorBCtrl = 0.0f;
00126            stby = 0;        // Turn off H-bridge driver
00127     }
00128 }
00129 
00130 /**
00131  * Sample encoders and find error on right wheel. Assume Left wheel is always 
00132  * correct speed.
00133  */
00134 void sampleEncoder()
00135 {
00136     // Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
00137     if(((strcmp(motorCmd, (char *)"fwrd") == 0) || 
00138             (strcmp(motorCmd, (char *)"back") == 0)) && (ticksL != 0 || ticksR != 0)) { 
00139         E = ticksL - ticksR; // Find error
00140         pc.printf("Error = %d\n\r", E);
00141         speedR = speedR + float(E) / 255.0f; // Assign a scaled increment to the right wheel based on error
00142         pc.printf("Updating right motor speed to %f\n\r", speedR);
00143         runMotor();
00144     }
00145     ticksR = 0; //Restart the counters
00146     ticksL = 0;
00147 }
00148 
00149 void dev_recv()
00150 {
00151     led1 = !led1;
00152     getreply();         // Get the serial message
00153     getMotorCommand();  // Extract the motor command from the message
00154     pc.printf(buf);
00155     // Check whether we are in emergency stop mode (robot is too close to a wall)
00156     if (emergencyStop == true && strcmp(motorCmd, (char *)"back") != 0) {
00157         // If so, only allow a "back" command
00158         strcpy(motorCmd, "stop");
00159     }
00160     runMotor();         // Run the motor based on the command
00161 }
00162  
00163 void pc_recv()
00164 {
00165     led4 = !led4;
00166     while(pc.readable()) {
00167         dev.putc(pc.getc());
00168     }
00169 }
00170 
00171 /**
00172  *  Do distance measurement
00173  */
00174 void distance_thread(void const *args) {
00175     SongPlayer speaker(p23);
00176     float alert1[2] = {440.0, 0.0};
00177     float alert1duration[2] = {0.05, 0.0};
00178     float alert2[2] = {880.0, 0.0};
00179     float alert2duration[2] = {0.05, 0.0};
00180     float alert3[2] = {1760.0, 0.0};
00181     float alert3duration[2] = {0.05, 0.0};
00182     while(1){
00183         float reading = irSensor;
00184         pc.printf("Distance: %f\n\r", reading);
00185         if (reading > 0.7f) {
00186             if (strcmp(motorCmd, (char *)"fwrd") == 0){
00187                 strcpy(motorCmd, "stop");
00188             }
00189             emergencyStop = true;
00190             runMotor();
00191         } else if (reading > 0.65f) {
00192             speaker.PlaySong(alert3, alert3duration);
00193             emergencyStop = false;
00194             Thread::wait(60);
00195         } else if (reading > 0.55f) {
00196             speaker.PlaySong(alert2, alert2duration);
00197             emergencyStop = false;
00198             Thread::wait(65);
00199         } else if (reading > 0.45f) {
00200             speaker.PlaySong(alert1, alert1duration);
00201             emergencyStop = false;
00202             Thread::wait(70);
00203         } else {
00204             Thread::wait(20);       // Wait 20 ms
00205         }
00206     }
00207 }
00208  
00209 int main()
00210 {
00211     pc.baud(115200);
00212     dev.baud(115200);
00213  
00214     pc.attach(&pc_recv, Serial::RxIrq);
00215     dev.attach(&dev_recv, Serial::RxIrq);
00216     
00217     // Initialize the motors
00218     motorACtrl.period(0.01f);
00219     motorBCtrl.period(0.01f);
00220     speedL = 0.9f;
00221     speedR = 0.9f;
00222     
00223     // Initialize hall-effect control
00224     Sampler.attach(&sampleEncoder, .1); //Sampler uses sampleEncoder function every 20ms
00225     EncL.mode(PullUp); // Use internal pullups
00226     EncR.mode(PullUp);
00227     
00228     Thread t0(distance_thread);
00229  
00230     while(1) {
00231         if((strcmp(motorCmd, (char *)"fwrd") == 0) || 
00232             (strcmp(motorCmd, (char *)"back") == 0)) { // Only increment tick values if moving forward or reverse
00233             int tmpL = EncL;    // Sample the current value of the encoders
00234             int tmpR = EncR;
00235             pc.printf("Encoder values %d, %d\n\r", tmpL, tmpR);
00236             if(tmpL != oldEncL) { // Increment ticks every time the state has switched. 
00237                 ticksL++;
00238                 oldEncL = tmpL;
00239             }
00240             if(tmpR != oldEncR) {
00241                 ticksR++;
00242                 oldEncR = tmpR;
00243             }
00244         }
00245         wait(0.02f);        // Sleep 20 ms
00246     }
00247 }