File content as of revision 1:50d97a0b4c64:

// Sweep the motor speed from full-speed reverse (-1.0) to full speed forwards (1.0)

#include "mbed.h"
#include "Motor.h"
#include "LSM9DS1.h"
#include "math.h"
#define PI 3.14159
#include "ultrasonic.h"
#include "Servo.h"
#include "SDFileSystem.h"
#include "wave_player.h"

Servo myservo(p23);

Motor m1(p22, p17, p15); // pwmA, fwd, rev, can brake
Motor m2(p21,p20,p19); //pwmB, fwd, rev,brake
Serial bluemod(p13,p14);
DigitalOut myLed(LED1);
Serial pc(USBTX, USBRX); // usb serial for debugging
SDFileSystem sd(p5, p6, p7, p8, "sd"); //SD card
AnalogOut DACout(p18);
wave_player waver(&DACout);
// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D 
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW
float mx;
float my;
float heading_val = 0.0;
#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
LSM9DS1 imu2(p28, p27, 0xD6, 0x3C);
float p = 0;
float calcHeading()
{
    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);
    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;
    return heading;
}


float updateHeading(){
        imu2.readMag();
        mx=imu2.mx;
        my=imu2.my;
        heading_val = calcHeading();
        pc.printf("%2f\r\n",abs(heading_val));
        return abs(heading_val);
  
  }

void playSounds(){
    FILE *wave_file;
    wave_file=fopen("/sd/blaster-firing.wav","r");
    waver.play(wave_file);
    fclose(wave_file);    
    
}
    
int main() {
    imu2.begin();
    if (!imu2.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    imu2.calibrate(1);
    wait(2);
    m1.speed(0.25);
    m2.speed(-0.25);
    imu2.calibrateMag(0);
    m1.speed(0.0);
    m2.speed(0.0);
    
    
    while(1){ 
        myLed = 0;
        wait(0.2);
        imu2.readMag();
        mx=imu2.mx;
        my=imu2.my;
        heading_val = calcHeading();
        heading_val = updateHeading();
        myservo.calibrate();
        if(bluemod.readable()){
                    float speed;
                    myLed = 1;
                    char bnum=0;
                    char bhit = 0;
                     if (bluemod.getc()=='!') {
                        if (bluemod.getc()=='B') { //button data
                            bnum = bluemod.getc(); //button number
                            bhit  = bluemod.getc();
                            switch(bnum){
                                case '1': // move forward
                                    if(bhit=='1'){
                                        speed=0.3;
                                        playSounds();
                                        }
                                    break;
                                case '2': // break
                                    if(bhit=='1'){
                                            speed=0;
                                        }
                                    break;
                                case '3':  //reverse
                                    if(bhit=='1'){
                                            speed=-0.3;
                                        }
                                    break;
                                case '4': //raise flag
                                    if(bhit=='1'){
                                        playSounds(); //play sound before raising flag
                                        myLed=1;
                                        myservo.write(0);
                                        wait(1);
                                        myLed =0;
                                        myservo.write(.05);
                                        wait(2);
                                        myLed = 1;
                                        m1.speed(0);
                                        m2.speed(0);
                                        playSounds();  //play sound after flag is raised
                                        }
                                    break;
                                case '5':  //north
                                    if(bhit=='1'){
                                        while(heading_val>5.0 && heading_val<355.0){
                                            heading_val = updateHeading();
                                            m1.speed(0.3);
                                            m2.speed(-0.3);
                                            }
                                        playSounds();
                                        m1.speed(0);
                                        m2.speed(0);
                                        }
                                    break;
                                case '6': //south: 175->185
                                    if(bhit=='1'){
                                       while(!(heading_val<185.0 && heading_val>175.0)){
                                            heading_val = updateHeading();
                                            m1.speed(0.3);
                                            m2.speed(-0.3);
                                            }
                                        playSounds();
                                        m1.speed(0);
                                        m2.speed(0);
                                        }
                                    break;
                                case '8': //east
                                    if(bhit=='1'){
                                       while(!(heading_val<95.0 && heading_val>85.0)){
                                            heading_val = updateHeading();
                                            m1.speed(0.3);
                                            m2.speed(-0.3);
                                            }
                                        playSounds();
                                        m1.speed(0);
                                        m2.speed(0);
                                        }
                                    break;
                                 case '7': //west
                                    if(bhit=='1'){
                                       while(!(heading_val<275.0 && heading_val>265.0)){
                                            heading_val = updateHeading();
                                            m1.speed(0.3);
                                            m2.speed(-0.3);
                                            }
                                        playSounds();//play sound when direction is found
                                        m1.speed(0);
                                        m2.speed(0);
                                        }
                                    break;
                            default: break;    
                            }//close case
                        } //close B
                } //!
        m1.speed(speed);
        m2.speed(speed);
        }//close readable
    }  // close while
}//main