File content as of revision 0:2266b80b252e:

#include "mbed.h"
#include "ultrasonic.h"
#include "SDFileSystem.h" 
#include <string>
#include "Motor.h"
#include "LSM9DS1.h"
#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
Serial pc(USBTX, USBRX);

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//H-bridge
Motor m_left(p21, p15, p16); // pwm,fwd,rev pwmA
Motor m_right(p22, p19, p20); // pwm,fwd,rev 

//Photo transistor
AnalogIn photocell(p18);

//Timer
Timer t; //t.start(), t.stop(), t.read()

//SD Card
SDFileSystem sd(p5, p6, p7, p8, "sd");

RawSerial  Bluetooth(p13,p14); // tx, rx

//Global variables for bluetooth control
bool sonarOn = false; //Indicates when sonar is taking measurements
char heading = '0'; //Indicates rough direction of the robot; 'f' = forward, 'l' = left, 'r' = right, 'b' = backwards, '0' = not moving
bool running = true; //Keeps main while loop going

   
//Code for interrupt routine for Bluetooth input
enum statetype {start = 0, got_exclm, got_B, got_one, got_two, got_three, got_five, got_six, got_seven, got_eight, got_11, got_21, got_31, got_51, got_61, got_71, got_81};
statetype state = start;
//Interrupt routine to parse message with one new character per serial RX interrupt
void parse_message()
{
    switch (state)
    {
        case start:
            if (Bluetooth.getc()=='!')
            {
                //led2 = 1;
                state = got_exclm;
            }
            else state = start;
            break;
        case got_exclm:
            
            if (Bluetooth.getc() == 'B')
            {
                state = got_B;
            }
            else state = start;
            break;
        case got_B:
        {
            char recv = Bluetooth.getc();
            if (recv == '1') state = got_one;
            else if (recv == '2') state = got_two;
            else if (recv == '3') state = got_three;
            else if (recv == '5') state = got_five;
            else if (recv == '6') state = got_six;
            else if (recv == '7') state = got_seven;
            else if (recv == '8') state = got_eight;
            else state = start;
        }
        break;
        case got_one:
            if (Bluetooth.getc() == '1')
            {
                //Make sure motors are stopped
                m_left.speed(0.0); 
                m_right.speed(0.0);
                sonarOn = false;
                heading = '0';
                running = false;
            }
            else state = start;
            break;
        case got_two: //Stop everything
            if (Bluetooth.getc() == '1')
            {
                
            }
            else state = start;
            break;  
        case got_three: //currently unimplemented
            if (Bluetooth.getc() == '1')
            {
                
            }
            else state = start;
            break;
        case got_five: //up arrow pressed
        {
            char recv = Bluetooth.getc();
            if (recv == '1') //Button pressed or held
            {
                sonarOn = true;
                heading = 'f';
                m_left.speed(0.42);
                m_right.speed(0.4);
            }
            else if (recv == '0') //Button released
            {
                //sonarOn = false;
                heading = '0';
                m_left.speed(0.0);
                m_right.speed(0.0);
            }
            else state = start;
        }
            break;      
        case got_six: //down arrow pressed
        {
            char recv = Bluetooth.getc();
            if (recv == '1') //Button pressed or held
            {
                sonarOn = true;
                heading = 'b';
                m_left.speed(-0.42);
                m_right.speed(-0.4);
            }
            else if (recv == '0')
            {
                //sonarOn = false;
                heading = '0';
                m_left.speed(0.0);
                m_right.speed(0.0);
            }
            else state = start;
        }
            break;    
        case got_seven: //turn left
        {
            char recv = Bluetooth.getc();
            //sonarOn = false;
            if (recv == '1')
            {
                heading = 'l';
                m_left.speed(-0.3);
                m_right.speed(0.3);
            }
            else if (recv == '0')
            {
                heading = '0';
                m_left.speed(0.0);
                m_right.speed(0.0);
            }
            else state = start;
        }
            break;  
        case got_eight: //turn right
        {
            char recv = Bluetooth.getc();
            //sonarOn = false;
            if (recv == '1')
            {
                heading = 'r';
                m_left.speed(0.3);
                m_right.speed(-0.3);
            }
            else if (recv == '0')
            {
                heading = '0';
                m_left.speed(0.0);
                m_right.speed(0.0);
            }
            else state = start;
        }
            break;  

        default:
            Bluetooth.getc();
            state = start;
    }
}

//To see serial output on Mac, run ls /dev/tty.usbmodem* once the mbed is connected
//to find its location. Then run screen <mbed location> to see serial output.
int Left[1000];
float Time[1000];
char Head[1000];
float Light[1000];
float Mag[1000];
int j = 0;
int k = 0;
volatile bool startTime = false;

void distL(int distance)
{
    if (sonarOn && startTime)
    {
        Left[j] = distance;
        Time[j] = t.read();
        Head[j] = heading;
        Light[j] = (float)photocell;
        j++;
        printf("Distance %i mm, Time %f s, Light %0.3f ,Heading %c \r\n", distance, t.read(), (float)photocell, heading);//code here to execute when distance changes
    }
}
//IMU
void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
    if(sonarOn && startTime)
    {
    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);
    wait(0.3);
    Mag[k] = heading;
    k++;
    }
}
//sample 10 times every 3 seconds
ultrasonic mu_L(p11, p12, .3, 1, &distL);    //Set the trigger pin to D8 and the echo pin to D9
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes
                                        
                                        
                                        
int main()
{
    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
    led = 0.0
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0);
    led1 = 1.0;
    wait(0.5);
    led1 = 0.0;
    //t.start(); //start timer
    mu_L.startUpdates(); //start measuring the distance with sonar sensor
    Bluetooth.attach(&parse_message,Serial::RxIrq); //attach bluetooth interrupt
    
    while(running)
    {
         mu_L.checkDistance();     //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
        //Start the timer only once sonar has turned on and the timer has not been previously started
        while(!IMU.tempAvailable());
        IMU.readTemp();
        while(!IMU.magAvailable(X_AXIS));
        IMU.readMag();
        while(!IMU.accelAvailable());
        IMU.readAccel();
        while(!IMU.gyroAvailable());
        IMU.readGyro();
        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));
        if (sonarOn && !startTime)
        {
            startTime = true;
            t.start();
            
        }
    }
    
    //Store values into SD card
    //Open a new file to store the data
    mkdir("/sd/mydir", 0777);
    FILE *left = fopen("/sd/mydir/left.txt", "w"); 
    if(left == NULL) //If there is an error in opening the file, throw error
    { 
        error("Could not open file left for write\n");
    }
    fprintf(left, "sonar, \t time, \t light, \t heading \n");
    //Loop through array values and store into each line of text
    for (int i=0; i<1000; i++) 
    {  
        
        fprintf(left, "%i, %0.4f, %0.4f, %0.4f, %c \n", Left[i], Time[i],Light[i] , Mag[i], Head[i]);
    }
    //Close file
    fclose(left);
}