File content as of revision 4:08a4ae8db71f:

#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);
        
    } */
    
    
}