Revision 15:7cafe0b078ea, committed 2017-05-02

Comitter:

CRaslawski

Date:

Tue May 02 06:11:03 2017 +0000

Parent:

14:088c1b04b4c1

Commit message:

Working RTOS 5 for Roomba!!

Changed in this revision

diff -r 088c1b04b4c1 -r 7cafe0b078ea main.cpp
--- a/main.cpp	Tue May 02 06:07:19 2017 +0000
+++ b/main.cpp	Tue May 02 06:11:03 2017 +0000
@@ -1,171 +1,266 @@
-#include "mbed.h"
-#include "rtos.h"
-#include "uLCD_4DGL.h"
-#include "ShiftBrite.h"
-#include "SDFileSystem.h"
-#include "wave_player.h"
-
-Mutex mutex;    //handles writing to the LCD
-Mutex mutex2;   //used for color changing
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
-Thread thread1;
-Thread thread2;
-Thread thread3;
-Thread thread4;
-DigitalOut latch(p15);
-DigitalOut enable(p16);
-SPI spi(p11, p12, p13);
-//uLCD_4DGL uLCD(p28,p27,p29); //(p27, p28, p30);    //tx, rx, rst
-uLCD_4DGL uLCD(p28, p27, p30); 
-ShiftBrite myBrite(p15,p16,spi); //latch, enable, spi
-SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
-AnalogOut DACout(p18);      //must be p18
-RawSerial BT(p9, p10);  //bluetooth pinout
-int red = 200;
-int blue = 0;
-int green = 0;
-FILE *wave_file = NULL;        //global bc its gotta be changed by Main while running in child thread
-FILE *wave_file2 = NULL;
-unsigned int redHex = 0xFF0000;
-unsigned int grnHex = 0x00FF00;
-unsigned int bluHex = 0x0000FF;
-
-
-wave_player waver(&DACout);
-
-void LCD_thread1() {
-    while(1){    
-        mutex.lock();
-        uLCD.filled_circle(64, 64, 12, 0xFF0000);
-        mutex.unlock();
-        wait(.5);
-        mutex.lock();
-        uLCD.filled_circle(64, 64, 12, bluHex);
-        mutex.unlock();
-        wait(.5);
-    }     
-}
-void LCD_thread2() {    //update a timer on the display every 100ms
-    uLCD.cls();
-    //float time = 0.0;
-    int count =0;
-    while(1) {
-        Thread::wait(60);
-        //time = time + 0.25;
-        mutex.lock();
-        uLCD.locate(0,0);
-        count++;
-        uLCD.printf("Counting! %i \n", count);
-        mutex.unlock();
-    }
-}
-
-void LED_thread() {
-    //flash red & blue for police siren
-    while(1){
-        mutex2.lock();
-        //myBrite.Write(red,green,blue);
-        myBrite.Write(150, green, 0);
-        mutex2.unlock();
-        wait(.5);
-        mutex2.lock();
-        myBrite.Write(0,green,150);
-        //myBrite.Write(blue,green,red);  // change LEDs so Red=OFF, Blue=ON
-        mutex2.unlock();
-        wait(.5); 
-    }
-} 
-
-void sound_thread(){
-    wave_file=fopen("/sd/Police_Siren.wav","r");    
-    if (wave_file == NULL){
-           led1=0;
-           led2=led3=led4 = 1;
-    }
-    waver.play(wave_file);
-    fclose(wave_file);  
-}
-
-void banker_thread(){
-    wave_file=fopen("/sd/banker_calling.wav","r");
-    if (wave_file == NULL) {
-        led1=led2=led3=1;
-        led4=0;
-    }
-    waver.play(wave_file);
-    fclose(wave_file);
-}
-
-int main() {
-    thread1.start(LED_thread);  //police lights work
-    thread2.start(LCD_thread1);
-    thread3.start(LCD_thread2);
-    thread4.start(sound_thread);
-    // use mutex to lock getc(), printf(), scanf()
-    // don't unlock until you've checked that it's readable() 
-    
-    char bnum=0;
-    
-    while(1) {
-        led3=0;
-        led4=1;
-        if (BT.getc()=='!') {
-            if (BT.getc()=='B') { //button data
-                bnum = BT.getc(); //button number
-                if (bnum == '1') {  //turn Green LED on
-                    green = 250; 
-                    led1 = 1;
-                    led2=led3=led4=0;
-                }
-                if (bnum == '2') { // revert to normal operation
-                    green = 0;
-                    bluHex = 0x0000FF;
-                    led1=led3=led4=0;
-                    led2=1;
-                    if(wave_file2 != NULL){
-                        thread4.terminate();
-                        fclose(wave_file2);
-                        thread4.start(sound_thread);
-                    }
-                }
-                if (bnum == '3') {  // change sound file playing  
-                    led2=led1=led4=0;
-                    led3=1;
-                    if ( wave_file != NULL) {
-                        led2=1;              // debug
-                        thread4.terminate(); //stop police siren from playing
-                        fclose(wave_file);
-                        led2=0;
-                        thread4.start(banker_thread);
-                    }
-                    
-                    
-                    //CHANGING THIS BLOCK TO ALLOW BANKER INTERRUPTION
-                    /*
-                    FILE *wave_file2;
-                    wave_file2=fopen("/sd/banker_calling.wav","r");
-                    if (wave_file2 == NULL){
-                        led1=led2=0;
-                        led3=led4=1;
-                    }
-                    waver.play(wave_file2);
-                    fclose(wave_file2);
-                    */
-                        
-                    //thread4.start(sound_thread); //return to siren
-                }
-                if (bnum == '4') {  // change LCD colors
-                    bluHex = 0x00FF00;  //change the lights to flash red/grn
-                    led2=led3=led1=0;
-                    led4=1;
-                }
-            }
-            led4=0;
-            led3=1;
-        }
-    } 
-
-}
\ No newline at end of file
+#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
+// your's here:
+// http://www.ngdc.noaa.gov/geomag-web/#declination
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+//collab test
+
+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
+// Heading calculations taken from this app note:
+// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
+void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+{
+    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)
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    //pc.printf("heading atan=%f \n\r",heading);
+    heading -= DECLINATION; //correct for geo location
+    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);
+}
+
+/*
+void dev_recv()
+{
+    led2 = !led2;
+    while(dev.readable()) {
+        pc.putc(dev.getc());
+    }
+}
+
+void pc_recv()
+{
+    led4 = !led4;
+    while(pc.readable()) {
+        dev.putc(pc.getc());
+    }
+}*/
+
+    // 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);   // 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());
+        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());
+        IMU.readGyro();
+        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));
+        myled = 1;
+        wait(0.5);
+        myled = 0;
+        wait(0.5);
+        
+    } */
+    
+    
+}
+