Diff: main.cpp

Revision:

3:a739432e805b

Parent:

2:516dd8a72972

Child:

4:08a4ae8db71f

--- a/main.cpp	Sun Apr 30 18:09:43 2017 +0000
+++ b/main.cpp	Mon May 01 21:59:37 2017 +0000
@@ -1,5 +1,10 @@
 #include "mbed.h"
 #include "LSM9DS1.h"
+#include "rtos.h"
+#include "SDFileSystem.h"
+#include "Motor.h"
+#include "wave_player.h"
+
 #define PI 3.14159
 // Earth's magnetic field varies by location. Add or subtract
 // a declination to get a more accurate heading. Calculate
@@ -7,12 +12,28 @@
 // http://www.ngdc.noaa.gov/geomag-web/#declination
 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
-//Serial pc(USBTX, USBRX);
-RawSerial  pc(USBTX, USBRX);
-RawSerial  dev(p28,p27); //tx, rx
+Serial pc(USBTX, USBRX);
+//RawSerial  pc(USBTX, USBRX);
+Serial dev(p28,p27);    // 
+//RawSerial  dev(p28,p27); //tx, rx
 DigitalOut myled(LED1);
 DigitalOut led2(LED2);
 DigitalOut led4(LED4);
+//IR sensors on p19(front) & p20 (right)
+AnalogIn IR1(p19);
+AnalogIn IR2(p20);
+//L and R DC motors
+Motor Left(p21, p14, p13);      // green wires. pwm, fwd, rev, add ", 1" for braking
+Motor Right(p22, p12, p11);     // red wires
+// Speaker out
+AnalogOut DACout(p18);      //must(?) be p18
+SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
+
+Thread thread1;
+Thread thread2;
+Mutex BTmutex;
+Mutex mutex;
+
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
@@ -22,7 +43,7 @@
 {
     float roll = atan2(ay, az);
     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
-// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+    // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
     mx = -mx;
     float heading;
     if (my == 0.0)
@@ -34,13 +55,10 @@
     if(heading>180.0) heading = heading - 360.0;
     else if(heading<-180.0) heading = 360.0 + heading;
     else if(heading<0.0) heading = 360.0  + heading;
-
-
     // Convert everything from radians to degrees:
     //heading *= 180.0 / PI;
     pitch *= 180.0 / PI;
     roll  *= 180.0 / PI;
-
     //~pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
     //~pc.printf("Magnetic Heading: %f degress\n\r",heading);
 }
@@ -62,12 +80,36 @@
     }
 }*/
 
-int main()
-{
+    // Driving Methods
+void forward(float speed){
+    Left.speed(speed);
+    Right.speed(speed);    
+}
+void reverse(float speed){
+    Left.speed(-speed);
+    Right.speed(-speed);
+}
+void turnRight(float speed){
+    Left.speed(speed);
+    Right.speed(-speed);
+    //wait(0.7);
+}
+void turnLeft(float speed){
+    Left.speed(-speed);
+    Right.speed(speed);
+    //wait(0.7);
+}
+void stop(){
+    Left.speed(0.0);
+    Right.speed(0.0);
+}
+
+void IMU(){
     //IMU setup
-    LSM9DS1 IMU(p9, p10, 0xd6, 0x3c);
-    IMU.begin();
-    if (!IMU.begin()) {
+    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
+    IMU.begin();           
+    if (!IMU.begin()) {    
+        led2=1;
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
@@ -81,9 +123,124 @@
     dev.attach(&dev_recv, Serial::RxIrq);*/
     
     while(1) {
-    
+        //myled = 1;
         while(!IMU.magAvailable(X_AXIS));
         IMU.readMag();
+        //myled = 0;
+        while(!IMU.accelAvailable());
+        IMU.readAccel();
+        while(!IMU.gyroAvailable());
+        IMU.readGyro();
+        BTmutex.lock();
+        pc.puts("        X axis    Y axis    Z axis\n\r");
+        dev.puts("        X axis    Y axis    Z axis\n\r");
+        pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
+        pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        BTmutex.unlock();
+        myled = 1;
+        wait(0.5);
+        myled = 0;
+        wait(0.5);   
+    }  
+}
+
+void defaultDrive(){        //default behavior for robot
+//Drive forward until object detected. Stop, turn left, then drive until IR2 says path is clear, then turn right to go around object.
+    forward(0.2);
+    if(IR1 > 0.85) {     // this is threshold for collision
+        stop();
+        Thread::wait(250);
+        // check if path to right is clear
+        if(IR2 < .4){
+            turnRight(0.3);
+            while(IR1>0.4){};   //turn until path in front is clear
+            stop();
+        }
+        else {
+            turnLeft(0.3);
+            while(IR1>0.4){};    //execute turn until front IR says path is clear
+                                // consider placing Thread::wait(??) within loop to account for IR polling?
+            stop();
+            //Thread::wait(250);
+        }
+        Thread::wait(250);
+        forward(0.2);
+        
+        /*PICK UP FROM HERE
+          Implement logic to control two scenarios:
+          1) Roomba has detected obstacle in front, but Right is clear. Has turned right and needs to continue driving
+          2) Roomba has detected obstacle, Right is blocked. Turn left & drive until Right is clear. Turn back to right (orig. Fwd heading) and continue.
+          2a) Consider more complex routing to circle around obstacle
+        */
+        
+        
+        /*
+        //while(IR2>0.5 && IR1<0.8){};     // drive until roomba has passed object.
+        while(IR1<0.8){};
+        stop();
+        Thread::wait(250);
+        //check that path in front is clear
+        if(IR1>0.8){        // if not clear, turn left again until front is clear
+            turnLeft(0.3);
+            while(IR1>0.4){}
+            stop();
+            Thread::wait(250);    
+            
+        }
+        else {
+            
+        }
+                
+        
+        Thread::wait(200);
+        
+        while(IR2>0.85 ) forward(0.3);   // drive until 
+        */
+        
+            
+        
+    }
+    
+}
+
+int main()
+{
+    thread1.start(IMU); // start the IMU thread
+    thread2.start(defaultDrive);
+    
+    if(IR > 0.85) {     // this is threshold for collision
+        stop();
+        turnLeft(0.3);
+        
+        
+    }
+    /*
+    //IMU setup
+    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);   // this executes. Pins are correct. Changing them causes fault
+    IMU.begin();           
+    if (!IMU.begin()) {    
+        led2=1;
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);
+    
+    //bluetooth setup
+    pc.baud(9600);
+    dev.baud(9600);
+
+    //pc.attach(&pc_recv, Serial::RxIrq);
+    //dev.attach(&dev_recv, Serial::RxIrq);
+    
+    while(1) {
+        //myled = 1;
+        while(!IMU.magAvailable(X_AXIS));
+        IMU.readMag();
+        //myled = 0;
         while(!IMU.accelAvailable());
         IMU.readAccel();
         while(!IMU.gyroAvailable());
@@ -92,7 +249,8 @@
         dev.puts("        X axis    Y axis    Z axis\n\r");
         //pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
         //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
-        //pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        dev.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
         myled = 1;
@@ -100,13 +258,8 @@
         myled = 0;
         wait(0.5);
         
-
-        //uLCD.filled_circle(floor(128*oldtempX), floor(128*oldtempY), 6, 0x000000);    // erase old bubble
-
-        //draw filled circle based on info from IMU
-        //draw new bubble
-        //uLCD.filled_circle(floor(128*tempX), floor(128*tempY), 6, 0xFFFFFF);
-        
-    }
+    } */
+    
+    
 }