Diff: main.cpp

Revision:

0:c29fc80c3ca3

Child:

1:6b8a201f4f90

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 02 19:22:12 2017 +0000
@@ -0,0 +1,225 @@
+#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);
+    
+    forward(0.3);
+    led4=1;
+    while(1){
+        
+    }
+    
+}
+