File content as of revision 1:de52879ff5ec:

#include "mbed.h"
#include "wave_player.h"
#include "SDFileSystem.h"
#include "Servo.h"
#include "ultrasonic.h"
#include "uLCD_4DGL.h"
#include "LSM9DS1.h"
#include "rtos.h"

Serial pc(USBTX, USBRX);
uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin;

DigitalOut led1(p26);
DigitalOut led2(p19);
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
Servo dcmotorBottom(p21); // pwm
Servo dcmotorBack(p22); // pwm
Servo servoBottom(p23); // pwm
AnalogOut DACout(p18);
wave_player waver(&DACout);
SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
Serial blue(p28,p27);
BusOut myleds(LED1,LED2,LED3,LED4);

char bnum=0;//for blue
Mutex my_mutex;
int dist_flag = 0;

void col_ESC() {
    dcmotorBottom = 0.0;
    dcmotorBack = 0.0;
    wait(0.5); //ESC detects signal
    //Required ESC Calibration/Arming sequence  
    //sends longest and shortest PWM pulse to learn and arm at power on
    dcmotorBottom = 1.0; //send longest PWM
    dcmotorBack = 1.0; //send longest PWM
    wait(8);
    dcmotorBottom = 0.0; //send shortest PWM
    dcmotorBack = 0.0; //send shortest PWM
    wait(8);
}

 void dist(int distance)
{
    while(1){
        //put code here to execute when the distance has changed
        printf("Distance %d mm\r\n", distance);
        if(distance <50){
            dist_flag=1;
        }
    }
    
}
ultrasonic mu(p15, p16, .1, 1, &dist); //sonar
#define PI 3.14159
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
double x2;
double y2;
int x_c;
int y_c;
int r;
int z;
float 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);
    return heading;
}

void display_compass()
{   
    while(!IMU.tempAvailable());
    IMU.readTemp();
    while(!IMU.magAvailable(X_AXIS));
    IMU.readMag();
    while(!IMU.accelAvailable());
    IMU.readAccel();
    while(!IMU.gyroAvailable());
    IMU.readGyro();
    pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
    pc.printf("        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));
    float heading = 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));
    double a = heading * 0.0174533; //deg to rad
    x2 = x_c + r*cos(a);
    y2 = y_c + r*sin(a);
    uLCD.cls();
    uLCD.circle(x_c, y_c, r, WHITE);
    uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
    wait(0.001);
}
void IMU_fun(void const *args){//thread 1
    x_c = 60;
    y_c = 60;
    r = 30;
    uLCD.circle(x_c, y_c, r, WHITE);
    z = 0;
    x2 = x_c + r*cos((double)z);
    y2 = x_c + r*sin((double)z);
    uLCD.line(x_c, y_c, (int)x2, (int)y2, BLUE);
    
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0);
    while(1){
        if(dist_flag==0){
            display_compass();
        }    
        
    }
}
void dcmotorBottom_fun(void const *args){//thread2
    while (1) {
        if(dist_flag ==0){
            for (float p=0.0; p<=0.9; p += 0.05) { //Throttle up slowly to full throttle
                dcmotorBottom = p;
                wait(1.0);
            }
        }
        if(dist_flag ==1){
            for (float p=0.9; p>=0.0; p -= 0.05) { //Throttle down slowly from full throttle
                dcmotorBottom = p;
                wait(1.0);
            }
        }
    }       
}
void dist_flag1(void const *args){//thread3
    while(1) {
         if(dist_flag==1){
            for(int i=0; i<4; ++i) {
                //open wav file and play it
                FILE *wave_file;
                printf("\n\n\nHello, wave world!\n");
                wave_file=fopen("/sd/beep.wav","r");
                waver.play(wave_file);
                fclose(wave_file);
                Thread::wait(1925);
            }
            led1 =!led1;
            led2 =!led2;
            wait(0.25);
        }
   
   
    }//while
}//fun

void Blue_control(void const *args){//thread4
    float servo_p = 0.5;
    while(1) {
        if(bnum=='1'){
            dist_flag =0;
        }
        if(dist_flag==0){
            if(bnum =='2'){
                for(int i=0; i<4; ++i) {
                    //open wav file and play it
                    FILE *wave_file;
                    printf("\n\n\nHello, wave world!\n");
                    wave_file=fopen("/sd/honk.wav","r");
                    waver.play(wave_file);
                    fclose(wave_file);
                    Thread::wait(1925);
                }
            }
            if(bnum=='3'){
                led1 =!led1;
                led2 =!led2;
            }
            if(bnum=='5'){   
                for (float p=0.0; p<=0.3; p += 0.025) { //Throttle up slowly to full throttle
                    dcmotorBack = p;
                    wait(1.0);
                }
            }
            if(bnum=='6'){   
                for (float p=0.3; p>=0.3; p -= 0.025) { //Throttle dowm slowly from full throttle
                    dcmotorBack = p;
                    wait(1.0);
                }
            }
            if(bnum=='7' && servo_p>0.0){
                servo_p = servo_p+0.05;
                servoBottom = servo_p;
            }
            if(bnum=='8' && servo_p<1.0){
                servo_p = servo_p-0.05;
                servoBottom = servo_p;
            }
        }
            
    }//while(1)
}//func





int main()//thread5
{
    mu.startUpdates();//start measuring the distance
    col_ESC();
    led1=0;
    led2=0;
    Thread t1(IMU_fun); //start thread1
    Thread t2(dcmotorBottom_fun); //start thread2
    Thread t3(dist_flag1); //start thread2
    Thread t4(Blue_control); //start thread2
    char bhit=0;//for blue
     while(1)
     {
        mu.checkDistance();
        my_mutex.lock();
        if (blue.getc()=='!') {
            if (blue.getc()=='B') { //button data packet
                bnum = blue.getc(); //button number
                bhit = blue.getc(); //1=hit, 0=release
                if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
                  my_mutex.unlock();
                    myleds = bnum - '0'; //current button number will appear on LEDs
                    
                    }
                }
            }
    }//for while(1)  
}//for int main()