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 
00008 #define PI 3.14159
00009 // Earth's magnetic field varies by location. Add or subtract
00010 // a declination to get a more accurate heading. Calculate
00011 // your's here:
00012 // http://www.ngdc.noaa.gov/geomag-web/#declination
00013 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00014 //collab test
00015 
00016 Serial pc(USBTX, USBRX);
00017 //RawSerial  pc(USBTX, USBRX);
00018 Serial dev(p28,p27);    // 
00019 //RawSerial  dev(p28,p27); //tx, rx
00020 DigitalOut myled(LED1);
00021 DigitalOut led2(LED2);
00022 DigitalOut led4(LED4);
00023 //IR sensors on p19(front) & p20 (right)
00024 AnalogIn IR1(p19);
00025 AnalogIn IR2(p20);
00026 //L and R DC motors
00027 Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
00028 Motor Right(p22, p12, p11);     // red wires
00029 // Speaker out
00030 AnalogOut DACout(p18);      //must(?) be p18
00031 SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
00032 
00033 Thread thread1;
00034 Thread thread2;
00035 Mutex BTmutex;
00036 Mutex mutex;
00037 
00038 // Calculate pitch, roll, and heading.
00039 // Pitch/roll calculations taken from this app note:
00040 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
00041 // Heading calculations taken from this app note:
00042 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
00043 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
00044 {
00045     float roll = atan2(ay, az);
00046     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
00047     // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00048     mx = -mx;
00049     float heading;
00050     if (my == 0.0)
00051         heading = (mx < 0.0) ? 180.0 : 0.0;
00052     else
00053         heading = atan2(mx, my)*360.0/(2.0*PI);
00054     //pc.printf("heading atan=%f \n\r",heading);
00055     heading -= DECLINATION; //correct for geo location
00056     if(heading>180.0) heading = heading - 360.0;
00057     else if(heading<-180.0) heading = 360.0 + heading;
00058     else if(heading<0.0) heading = 360.0  + heading;
00059     // Convert everything from radians to degrees:
00060     //heading *= 180.0 / PI;
00061     pitch *= 180.0 / PI;
00062     roll  *= 180.0 / PI;
00063     //~pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
00064     //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
00065 }
00066 
00067 /*
00068 void dev_recv()
00069 {
00070     led2 = !led2;
00071     while(dev.readable()) {
00072         pc.putc(dev.getc());
00073     }
00074 }
00075 
00076 void pc_recv()
00077 {
00078     led4 = !led4;
00079     while(pc.readable()) {
00080         dev.putc(pc.getc());
00081     }
00082 }*/
00083 
00084     // Driving Methods
00085 void forward(float speed){
00086     Left.speed(speed);
00087     Right.speed(speed);    
00088 }
00089 void reverse(float speed){
00090     Left.speed(-speed);
00091     Right.speed(-speed);
00092 }
00093 void turnRight(float speed){
00094     Left.speed(speed);
00095     Right.speed(-speed);
00096     //wait(0.7);
00097 }
00098 void turnLeft(float speed){
00099     Left.speed(-speed);
00100     Right.speed(speed);
00101     //wait(0.7);
00102 }
00103 void stop(){
00104     Left.speed(0.0);
00105     Right.speed(0.0);
00106 }
00107 
00108 void IMU(){
00109     //IMU setup
00110     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
00111     IMU.begin();           
00112     if (!IMU.begin()) {    
00113         led2=1;
00114         pc.printf("Failed to communicate with LSM9DS1.\n");
00115     }
00116     IMU.calibrate(1);
00117     IMU.calibrateMag(0);
00118     
00119     //bluetooth setup
00120     pc.baud(9600);
00121     dev.baud(9600);
00122 
00123     /*pc.attach(&pc_recv, Serial::RxIrq);
00124     dev.attach(&dev_recv, Serial::RxIrq);*/
00125     
00126     while(1) {
00127         //myled = 1;
00128         while(!IMU.magAvailable(X_AXIS));
00129         IMU.readMag();
00130         //myled = 0;
00131         while(!IMU.accelAvailable());
00132         IMU.readAccel();
00133         while(!IMU.gyroAvailable());
00134         IMU.readGyro();
00135         BTmutex.lock();
00136         pc.puts("        X axis    Y axis    Z axis\n\r");
00137         dev.puts("        X axis    Y axis    Z axis\n\r");
00138         pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00139         pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00140         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00141         dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00142         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00143                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00144         BTmutex.unlock();
00145         myled = 1;
00146         wait(0.5);
00147         myled = 0;
00148         wait(0.5);   
00149     }  
00150 }
00151 
00152 void defaultDrive(){        //default behavior for robot
00153 //Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
00154     forward(0.2);
00155     if(IR1 > 0.85) {     // this is threshold for collision
00156         stop();
00157         Thread::wait(250);
00158         // check if path to right is clear
00159         if(IR2 < .4){
00160             turnRight(0.3);
00161             while(IR1>0.4){};   //turn until path in front is clear
00162             stop();
00163         }
00164         else {
00165             turnLeft(0.3);
00166             while(IR1>0.4){};    //execute turn until front IR says path is clear
00167                                 // consider placing Thread::wait(??) within loop to account for IR polling?
00168             stop();
00169             //Thread::wait(250);
00170         }
00171         Thread::wait(250);
00172         forward(0.2);
00173         
00174         /*PICK UP FROM HERE
00175           Implement logic to control two scenarios:
00176           1) Roomba has detected obstacle in front, but Right is clear. Has turned right and needs to continue driving
00177           2) Roomba has detected obstacle, Right is blocked. Turn left & drive until Right is clear. Turn back to right (orig. Fwd heading) and continue.
00178           2a) Consider more complex routing to circle around obstacle
00179         */
00180         
00181         
00182         /*
00183         //while(IR2>0.5 && IR1<0.8){};     // drive until roomba has passed object.
00184         while(IR1<0.8){};
00185         stop();
00186         Thread::wait(250);
00187         //check that path in front is clear
00188         if(IR1>0.8){        // if not clear, turn left again until front is clear
00189             turnLeft(0.3);
00190             while(IR1>0.4){}
00191             stop();
00192             Thread::wait(250);    
00193             
00194         }
00195         else {
00196             
00197         }
00198                 
00199         
00200         Thread::wait(200);
00201         
00202         while(IR2>0.85 ) forward(0.3);   // drive until 
00203         */
00204         
00205             
00206         
00207     }
00208     
00209 }
00210 
00211 int main()
00212 {
00213     thread1.start(IMU); // start the IMU thread
00214     thread2.start(defaultDrive);
00215     
00216     if(IR > 0.85) {     // this is threshold for collision
00217         stop();
00218         turnLeft(0.3);
00219         
00220         
00221     }
00222     /*
00223     //IMU setup
00224     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
00225     IMU.begin();           
00226     if (!IMU.begin()) {    
00227         led2=1;
00228         pc.printf("Failed to communicate with LSM9DS1.\n");
00229     }
00230     IMU.calibrate(1);
00231     IMU.calibrateMag(0);
00232     
00233     //bluetooth setup
00234     pc.baud(9600);
00235     dev.baud(9600);
00236 
00237     //pc.attach(&pc_recv, Serial::RxIrq);
00238     //dev.attach(&dev_recv, Serial::RxIrq);
00239     
00240     while(1) {
00241         //myled = 1;
00242         while(!IMU.magAvailable(X_AXIS));
00243         IMU.readMag();
00244         //myled = 0;
00245         while(!IMU.accelAvailable());
00246         IMU.readAccel();
00247         while(!IMU.gyroAvailable());
00248         IMU.readGyro();
00249         pc.puts("        X axis    Y axis    Z axis\n\r");
00250         dev.puts("        X axis    Y axis    Z axis\n\r");
00251         //pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
00252         //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
00253         pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00254         dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00255         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
00256                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
00257         myled = 1;
00258         wait(0.5);
00259         myled = 0;
00260         wait(0.5);
00261         
00262     } */
00263     
00264     
00265 }
00266