main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "rtos.h"
00004 //#include "SDFileSystem.h"
00005 #include "Motor.h"
00006 //#include "wave_player.h"
00007 #include "ultrasonic.h"
00008 
00009 #define PI 3.14159
00010 // Earth's magnetic field varies by location. Add or subtract
00011 // a declination to get a more accurate heading. Calculate
00012 // your's here:
00013 // http://www.ngdc.noaa.gov/geomag-web/#declination
00014 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00015 //collab test
00016 
00017 Serial pc(USBTX, USBRX);
00018 //RawSerial  pc(USBTX, USBRX);
00019 Serial dev(p28,p27);    //
00020 //RawSerial  dev(p28,p27); //tx, rx
00021 DigitalOut myled(LED1);
00022 DigitalOut led2(LED2);
00023 DigitalOut led3(LED3);
00024 DigitalOut led4(LED4);
00025 //IR sensors on p19(front) & p20 (right)
00026 AnalogIn IR1(p19);
00027 AnalogIn IR2(p20);
00028 //L and R DC motors
00029 Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
00030 Motor Right(p22, p12, p11);     // red wires
00031 // Speaker out
00032 //AnalogOut DACout(p18);      //must(?) be p18
00033 //SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
00034 
00035 Thread thread1;
00036 //Thread thread2;
00037 //Mutex BTmutex;                  //mutex for send/recv data on Bluetooth
00038 Mutex mutex;                    //other mutex for global resources
00039 
00040 //Globals
00041 float throttle = 0.5;
00042 //float currIR1;
00043 float currIR2;
00044 float sonar;
00045 float sonarThresh = 0.5;
00046 //float origHeading;
00047 //float heading;
00048 
00049 void dist(int distance)
00050 {
00051     //put code here to execute when the distance has changed
00052     if(distance*0.00328084 < 40) {
00053     //printf("Distance %f ft\r\n", distance*0.00328084);
00054     }
00055 }
00056 
00057 ultrasonic mu(p29, p30, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
00058                                         //have updates every .1 seconds and a timeout after 1
00059                                         //second, and call dist when the distance changes
00060 
00061 // Calculate pitch, roll, and heading.
00062 // Pitch/roll calculations taken from this app note:
00063 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00064 // Heading calculations taken from this app note:
00065 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00066 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00067 {
00068     //entire subroutine is BTmutexed already
00069     float roll = atan2(ay, az);
00070     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00071     // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00072     mx = -mx;
00073     float heading;        //was global
00074     if (my == 0.0) {
00075         //mutex.lock(); //heading isn't global mutexes not needed
00076         heading = (mx < 0.0) ? 180.0 : 0.0;
00077         //mutex.unlock();
00078     } else {
00079         //mutex.lock();
00080         heading = atan2(mx, my)*360.0/(2.0*PI);
00081         //mutex.unlock();
00082     }
00083     //pc.printf("heading atan=%f \n\r",heading);
00084     //mutex.lock();
00085     heading -= DECLINATION; //correct for geo location
00086     if(heading>180.0) heading = heading - 360.0;
00087     else if(heading<-180.0) heading = 360.0 + heading;
00088     else if(heading<0.0) heading = 360.0  + heading;
00089     //mutex.unlock();
00090     // Convert everything from radians to degrees:
00091     //heading *= 180.0 / PI;
00092     pitch *= 180.0 / PI;
00093     roll  *= 180.0 / PI;
00094     mutex.lock();
00095     //~pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00096     //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
00097     //dev.printf("Magnetic Heading: %f degrees\n\r",heading);
00098     mutex.unlock();
00099 }
00100 
00101 /*
00102 void dev_recv()
00103 {
00104     led2 = !led2;
00105     while(dev.readable()) {
00106         pc.putc(dev.getc());
00107     }
00108 }
00109 void pc_recv()
00110 {
00111     led4 = !led4;
00112     while(pc.readable()) {
00113         dev.putc(pc.getc());
00114     }
00115 }*/
00116 
00117 // Driving Methods
00118 void forward(float speed)
00119 {
00120     Left.speed(speed);
00121     Right.speed(speed);
00122 }
00123 void reverse(float speed)
00124 {
00125     Left.speed(-speed);
00126     Right.speed(-speed);
00127 }
00128 void turnRight(float speed)
00129 {
00130     Left.speed(speed);
00131     Right.speed(-speed);
00132     //wait(0.7);
00133 }
00134 void turnLeft(float speed)
00135 {
00136     Left.speed(-speed);
00137     Right.speed(speed);
00138     //wait(0.7);
00139 }
00140 void stop()
00141 {
00142     Left.speed(0.0);
00143     Right.speed(0.0);
00144 }
00145 
00146 void IMU()
00147 {
00148     //IMU setup
00149     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
00150     IMU.begin();
00151     if (!IMU.begin()) {
00152         led2=1;
00153         pc.printf("Failed to communicate with LSM9DS1.\n");
00154     }
00155     IMU.calibrate(1);
00156     IMU.calibrateMag(0);
00157 
00158     while(1) {
00159         //myled = 1;
00160         while(!IMU.magAvailable(X_AXIS));
00161         IMU.readMag();
00162         //myled = 0;
00163         while(!IMU.accelAvailable());
00164         IMU.readAccel();
00165         while(!IMU.gyroAvailable());
00166         IMU.readGyro();
00167         //mutex.lock(); //changed from BTmutex
00168         //pc.puts("        X axis    Y axis    Z axis\n\r");
00169         //dev.puts("        X axis    Y axis    Z axis\n\r");
00170         //pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00171         //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00172         //pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00173         //dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00174         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00175                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00176         //mutex.unlock(); //changed from BTmutex
00177         //myled = 1;
00178         wait(0.5);
00179         //myled = 0;
00180         wait(0.5);
00181     }
00182 }
00183 
00184 void avoidObstacle()
00185 {
00186     //currIR1 = IR1;        //get IR readings - already received from main thread that initially decided to call avoidObstacle()
00187     //currIR2 = IR2;
00188     stop();
00189     Thread::wait(300);
00190     //BTmutex.lock();
00191     //dev.printf("Collision Detected!\n\r");
00192     //BTmutex.unlock();
00193     //dev.printf("Turning Left...\n\r");
00194     turnLeft(0.4);          //turn 90deg
00195     Thread::wait(1000);      //time to turn estimate
00196     stop();
00197     Thread::wait(500);
00198     // turn should be complete. Drive until obstacle passed on right, then turn right again
00199     //BTmutex.lock();
00200     //dev.printf("Driving past obstacle.\n\r");
00201     //BTmutex.unlock();
00202     forward(throttle);
00203     bool objOnRight = true;
00204     while(objOnRight) {
00205         mutex.lock();
00206         //pc.printf("Avoiding Obstacles...\n\r");
00207         //currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
00208         sonar = mu.getCurrentDistance()*0.00328084;
00209         currIR2 = IR2;
00210         //pc.printf("    IR1 Reading         IR2 Reading\n\r         %f          %f\n\r", sonar, currIR2);
00211         mutex.unlock();
00212         if(currIR2 < 0.7) {
00213             objOnRight = false;   //if IR2 drops below threshold, obstacle passed. Break out of loop
00214             
00215             wait(0.5);              //give robot time to drive past object
00216         }
00217         if(sonar < sonarThresh){          // don;t crash to anything in front
00218             stop();
00219             myled=led2=led3=led4=1;
00220         }
00221         //Thread::wait(1250); //
00222     }
00223     stop();
00224     Thread::wait(250);
00225     //BTmutex.lock();
00226     //dev.printf("Object passed. Turning right...\n\r");
00227     turnRight(0.5);     // turn 90deg
00228     Thread::wait(1000);      //time to turn estimate
00229     stop();
00230     Thread::wait(1000);
00231     forward(throttle);
00232 }
00233 
00234 /*
00235 void defaultDrive()         //default behavior for robot //moved to main instead of being a thread
00236 {
00237     //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
00238     forward(throttle);
00239     while(1) {
00240         myled=1;
00241         //update current IR readings
00242         currIR1 = IR1;
00243         currIR2 = IR2;
00244         BTmutex.lock();         //prevent race conditions in BT dataoutput
00245         //dev.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT
00246         //dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
00247         pc.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over BT
00248         pc.printf("        %2f             %2f\n\r", currIR1, currIR2);
00249 
00250         BTmutex.unlock();
00251         // Forward collision handling code block
00252         if(currIR1 > 0.8) {     // 0.85 is threshold for collision
00253             led3=1;
00254             avoidObstacle();    // steer around obstacle when detected
00255             led3=0;
00256         }
00257         Thread::wait(400);      // for debug. IR polling too quick and floods output terminal
00258         wait(0.4);
00259         myled=0;
00260     }
00261 }
00262 */
00263 
00264 /*
00265 void manualMode() // also moved to main
00266 {
00267     bool on = true;
00268     char temp;
00269     while(on) {
00270         temp = dev.getc();
00271         if(temp == 'A') { // reset command
00272             on = false;
00273         } else if(temp=='U') {
00274             led2=led3=1;
00275             forward(throttle);
00276             wait(1);
00277             led2=led3=0;
00278         } else if(temp=='L') { // turn left
00279             myled=led2=1;   //debug
00280             stop();
00281             wait(0.3);
00282             turnLeft(0.4);
00283             wait(0.6);
00284             stop();
00285             wait(0.3);
00286             forward(throttle);
00287             myled=led2=0;   //debug
00288         } else if(temp=='R') { // turn right
00289             led3=led4=1;
00290             stop();
00291             wait(0.3);
00292             turnRight(0.4);
00293             wait(0.6);
00294             stop();
00295             wait(0.3);
00296             forward(throttle);
00297             led3=led4=0;
00298         } else if(temp=='X') { // halt/brake command
00299             stop();
00300         }
00301         //myled=1;
00302         //wait(0.5);
00303         //myled=0;
00304         //wait(0.5);
00305     }
00306 }
00307 */
00308 
00309 /*
00310 void updateIRs()
00311 {
00312     mutex.lock();
00313     currIR1 = IR1; //must keep scanning IR readers to know when object is cleared
00314     currIR2 = IR2;
00315     mutex.unlock();
00316 }*/
00317 
00318 int main()
00319 {
00320     //bluetooth setup
00321     pc.baud(9600);
00322     dev.baud(9600);
00323 
00324     mu.startUpdates();//start measuring the distance from Sonar
00325     //wait to recv start command or time delay
00326     for(int i=0; i<3; i++) {        //temp delay for a few sec
00327         myled=led2=led3=led4=1;
00328         wait(0.5);
00329         myled=led2=led3=led4=0;
00330         wait(0.5);
00331     }
00332     thread1.start(IMU); // start the IMU thread
00333     char state = 'D'; //Roomba's drive state
00334     char temp;
00335     /*
00336     while(1){   //robot will receive a char from GUI signalling time to start
00337         temp = dev.getc();
00338         led3=1;
00339         pc.putc(temp);
00340         if (temp == 'B'){
00341             break;
00342         }
00343         if(led2 == 0) led2 = 1;
00344         else {led2 = 0;}
00345         wait(0.25);
00346     }
00347     */
00348     led3=0;
00349     //thread2.start(defaultDrive); default drive inserted into main while
00350     while(1) {
00351         //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
00352         forward(throttle);
00353         while(state == 'D') { //default drive
00354             myled=1;
00355             //update current IR readings
00356             //mutex.lock();//IR readings included in mutex since they are shared global variables
00357             //currIR1 = IR1; //replaced with sonar
00358             mu.checkDistance();
00359             sonar = mu.getCurrentDistance()*0.00328084;
00360             currIR2 = IR2;
00361             mutex.lock();         //prevent race conditions in BT dataoutput //changed from BTmutex
00362             //pc.puts("      Front Sonar reading    Right IR reading\n\r");     // print IR readings over BT
00363             //dev.printf("        %2f             %2f\n\r", currIR1, currIR2);
00364             //pc.printf("        %2f             %2f\n\r", sonar, currIR2); //changed
00365             //pc.puts("      Front IR reading    Right IR reading\n\r");     // print IR readings over serial
00366             //pc.printf("        %2f             %2f\n\r", currIR1, currIR2);
00367             mutex.unlock(); // changed from BTmutex
00368             
00369             // Forward collision handling code block
00370             if(sonar < sonarThresh) {     // 0.85 is threshold for collision
00371                 led3=1;
00372                 avoidObstacle();    // steer around obstacle when detected
00373                 led3=0;
00374             }
00375             Thread::wait(400);      // for debug. IR polling too quick and floods output terminal
00376             wait(0.4);
00377             myled=0;
00378             
00379             //was already ITT
00380             if (dev.readable()) {
00381                 mutex.lock();
00382                 temp = dev.getc();
00383                 pc.putc(temp);
00384                 mutex.unlock();
00385             }
00386             if(temp == 'M') {
00387                 led4=1;
00388                 stop();
00389                 //thread2.terminate();     //stop default drive
00390                 //manualMode();       //switch to manual control
00391                 /*
00392                 while(1) {
00393                     temp = dev.getc();
00394                     if(temp=='U') {
00395                         led2=1;
00396                     }
00397                 }   */
00398                 //once manualMode is exited, return to default
00399                 led4=0;
00400                 //thread2.start(defaultDrive);
00401                 state = 'M';
00402             }
00403         }
00404 
00405         while(state == 'M') {
00406             if (dev.readable()){
00407                 mutex.lock();
00408                 temp = dev.getc();
00409                 pc.putc(temp);
00410                 mutex.unlock();
00411             }
00412             if(temp == 'A') { // reset command
00413                 state = 'D';
00414             } else if(temp=='U') {
00415                 led2=led3=1;
00416                 forward(throttle);
00417                 wait(1);
00418                 led2=led3=0;
00419             } else if(temp=='L') { // turn left
00420                 myled=led2=1;   //debug
00421                 stop();
00422                 wait(0.5);
00423                 turnLeft(0.4);
00424                 wait(1);
00425                 stop();
00426                 wait(0.5);
00427                 forward(throttle);
00428                 myled=led2=0;   //debug
00429             } else if(temp=='R') { // turn right
00430                 led3=led4=1;
00431                 stop();
00432                 wait(0.3);
00433                 turnRight(0.5);
00434                 wait(0.6);
00435                 stop();
00436                 wait(0.3);
00437                 forward(throttle);
00438                 led3=led4=0;
00439             } else if(temp=='X') { // halt/brake command
00440                 stop();
00441             }
00442             //myled=1;
00443             //wait(0.5);
00444             //myled=0;
00445             //wait(0.5);
00446         }
00447     }
00448 }