File content as of revision 0:d2f4489d6f31:

#include "mbed.h"
#include "rtos.h" //mbed's thread and mutex are included here
#include <string>
#include "SDFileSystem.h"
#include "Motor.h"
#include "BNO055.h"
#include "ultrasonic.h"

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

//Semaphore s(1);
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 

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

//Sonar 1 & 2
//void newdist();
//ultrasonic mu1(p29, p30, .07, 1, &newdist); 
//ultrasonic mu2(p11, p12, .07, 1, &newdist);

//Bluetooth
// don't forgot to use mutex and volatile
//RawSerial  pc(USBTX, USBRX);
/*
1 for imu rotation
2 for reverse
3 for forward
4 for left
5 for right
*/
Serial pc(USBTX, USBRX);
RawSerial  Bluetooth(p28,p27);

//Global variables for bluetooth control
bool sonarOn = false; //Indicates when sonar is taking measurements
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()
{
    led1 = !led1;
    switch (state)
    {
        case start:
            if (Bluetooth.getc()=='!')
            {
                led2 = 1;
                state = got_exclm;
            }
            else state = start;
            break;
        case got_exclm:
            
            if (Bluetooth.getc() == 'B')
            {
                led3 = 1;
                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')
            {
                
            }
            else state = start;
            break;
        case got_two: //Stop everything
            if (Bluetooth.getc() == '1')
            {
                //Make sure motors are stopped
                m_left.speed(0.0); 
                m_right.speed(0.0);
                sonarOn = false;
                running = false;
            }
            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;
                m_left.speed(0.3);
                m_right.speed(0.3);
                led3 = 1;
            }
            else if (recv == '0') //Button released
            {
                sonarOn = false;
                m_left.speed(0.0);
                m_right.speed(0.0);
                led4 = 1;
            }
            else state = start;
        }
            break;      
        case got_six: //down arrow pressed
        {
            char recv = Bluetooth.getc();
            if (recv == '1') //Button pressed or held
            {
                sonarOn = true;
                m_left.speed(-0.3);
                m_right.speed(-0.3);
            }
            else if (recv == '0')
            {
                sonarOn = false;
                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')
            {
                m_left.speed(-0.3);
                m_right.speed(0.3);
            }
            else if (recv == '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')
            {
                m_left.speed(0.3);
                m_right.speed(-0.3);
            }
            else if (recv == '0')
            {
                m_left.speed(0.0);
                m_right.speed(0.0);
            }
            else state = start;
        }
            break;  

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


//phototransistor and IC





//Function to run in thread for speaker music    


void newDist()
{
    
}
    


int main() 
{
    
    //Thread t1;
    //Thread t2;
    //Thread t3;
    //t1.set_priority(osPriorityHigh);
    //t2.set_priority(osPriorityLow);
    //t3.set_priority(osPriorityRealtime);

    
    //Thread t4; 
    //t4.start(parse_direction);
    
    //Attach Bluetooth interrupt routine
    Bluetooth.attach(&parse_message,Serial::RxIrq);

    while (1)
    {
        //printf("print\t");
        Thread::wait(1000);
    }
    
    
    
}