/4180controllerdemo/main.cpp

#include "mbed.h"
#include "Motor.h"
#include "SDFileSystem.h"
#include "wave_player.h"
#include "SongPlayer.h"
//#include "uLCD_4DGL.h"
#include "LSM9DS1.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.

Serial pc(USBTX, USBRX);

Motor a(p21, p20, p19); // pwm, fwd, rev
Motor b(p22, p17, p16); // pwm, fwd, rev

BusOut myled(LED1,LED2,LED3,LED4);
Serial blue(p28,p27);

AnalogOut speaker(p18);
wave_player waver(&speaker);
SDFileSystem sd(p5, p6, p7, p8, "sd");

int main()
{
   LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //start IMU code
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    IMU.calibrateMag(0); 
    int ax=0, ay=0, hx=0, hy=0;
    float mx=0, my=0, heading=0; //end IMU code
    float sL = 0.0;
    float sR = 0.0;
    char bnum=0;
    char bhit=0;
    while(1) {
        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?
                    myled = bnum - '0'; //current button number will appear on LEDs
                    switch (bnum) {
                        case '1': //number button 1
                            if (bhit=='1') {
                                //stop
                                sL = 0.0; sR = 0.0;
                            } else {
                                //add release code here
                            }
                            break;
                        case '2': //number button 1
                            if (bhit=='1') {
                                //stop
                                FILE *wave_file;
                                wave_file=fopen("/sd/walker-grace-bitch.wav","r");
                                waver.play(wave_file);
                                fclose(wave_file);
                            } else {
                                //add release code here
                            }
                            break;
                        case '3': //number button 1
                            if (bhit=='1') {
                                while(!IMU.tempAvailable()); //begin IMU read
                                    IMU.readTemp();
                                    while(!IMU.magAvailable(X_AXIS));
                                    IMU.readMag();
                                    while(!IMU.accelAvailable());
                                    IMU.readAccel();
                                    while(!IMU.gyroAvailable());
                                    IMU.readGyro();
                                    //read X,Y +/-Gs and scale for #display pixels
                                    ax = (ax + IMU.calcAccel(IMU.ax) * 32.0)/2.0;
                                    ay = (ay -(IMU.calcAccel(IMU.ay) * 16.0))/2.0;
                                    mx = (float) IMU.mx;
                                    my = (float) IMU.my;
                                    if (my > 0) heading = 90 - (atan(mx / my) * (180 / 3.14)); //calculate heading
                                    else if (my < 0) heading = - (atan(mx / my) * (180 / 3.14));
                                    else // hy = 0
                                    {
                                        if (mx < 0) heading = 180;
                                        else heading = 0;
                                    }
                                    hx = (int)(17*mx);
                                    hy = (int)(17*my); 
                                    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)); 
                            } else {
                                //add release code here
                            }
                            break;
                        case '5': //button 5 up arrow
                            if (bhit=='1') {
                                //forwards
                                sL = 0.75; sR = 0.75;
                            } else {
                                //add release code here
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                //backwards
                                sL = -0.75; sR = -0.75;
                            } else {
                                //add release code here
                            }
                            break;
                        case '7': //button 7 left arrow
                            if (bhit=='1') {
                                //turn Left
                                sL = -0.25; sR = 0.75;
                            } else {
                                //add release code here
                            }
                            break;
                        case '8': //button 8 right arrow
                            if (bhit=='1') {
                                //turn right
                                sL = 0.75; sR = -0.25;
                            } else {
                                //add release code here
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
        }
       a.speed(sL);
       b.speed(sR);
        wait(0.2);
    } //end IMU read and while loop
}