File content as of revision 1:036b11214212:

#include "mbed.h"
#include "LSM9DS1.h"
#define PI 3.14159
#include "uLCD_4DGL.h"
#include "mbed.h"

Serial xbee1(p13, p14);
DigitalOut rst1(p11);
Serial pc(USBTX, USBRX);
DigitalOut myled(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
BusOut mbedleds(LED1,LED2,LED3,LED4);
//BusOut/In is faster than multiple DigitalOut/Ins
 
class Nav_Switch
{
public:
    Nav_Switch(PinName up,PinName down,PinName left,PinName right,PinName fire);
    int read();
//boolean functions to test each switch
    bool up();
    bool down();
    bool left();
    bool right();
    bool fire();
//automatic read on RHS
    operator int ();
//index to any switch array style
    bool operator[](int index) {
        return _pins[index];
    };
private:
    BusIn _pins;
 
};
Nav_Switch::Nav_Switch (PinName up,PinName down,PinName left,PinName right,PinName fire):
    _pins(up, down, left, right, fire)
{
    _pins.mode(PullUp); //needed if pullups not on board or a bare nav switch is used - delete otherwise
    wait(0.001); //delays just a bit for pullups to pull inputs high
}
inline bool Nav_Switch::up()
{
    return !(_pins[0]);
}
inline bool Nav_Switch::down()
{
    return !(_pins[1]);
}
inline bool Nav_Switch::left()
{
    return !(_pins[2]);
}
inline bool Nav_Switch::right()
{
    return !(_pins[3]);
}
inline bool Nav_Switch::fire()
{
    return !(_pins[4]);
}
inline int Nav_Switch::read()
{
    return _pins.read();
}
inline Nav_Switch::operator int ()
{
    return _pins.read();
}
 
Nav_Switch myNav(p29,p26,p27,p25, p28); //pin order on Sparkfun breakout

float y=0;
float z=0;

int main()
{
     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    
    wait(3);
    rst1 = 0;   //Set reset pin to 0
    myled = 1;
    //myled2= 0;
    wait_ms(1);
    rst1 = 1;   //Set reset pin to 1
    wait_ms(1);
   
    while(1) {
      int a;//getKeyNum();      
       int b;
       while(!IMU.accelAvailable());
        IMU.readAccel();
   
        //These conditional statements create a "dead-zone" for the wheel, to make it less
        //sensitive and more usable. Additionally, the IMU data will never exceed .9 to so
        //as to limit the maximum speed and steering for the robot.
        if(IMU.calcAccel(IMU.ay)<-.1) {
            b = 1;
            y=.9;
            a = 4;
            wait(.5);
        } else if (IMU.calcAccel(IMU.ay)>.1) {
            b = 1;
            y=-.9;
            a = 3;
            wait(.5);
        //} else if(IMU.calcAccel(IMU.ay)<=.1||IMU.calcAccel(IMU.ay)>=-.1) {
        //    y=IMU.calcAccel(IMU.ay);
            
        } else if(IMU.calcAccel(IMU.ax)>.1) {
            b = 1;
            z=.9;
            a = 2;
            wait(.5);
        } else if((IMU.calcAccel(IMU.ax)<-.1)) {
            b = 1;
            z=IMU.calcAccel(IMU.az);;
            a = 1;
            wait(.5);
        } else {
            a = 0;
            b = 0;
        } 
       
        if (b == 0) {
        //with pullups a button hit is a "0" - "~" inverts data to leds
        mbedleds = ~(myNav & 0x0F); //update leds with nav switch direction inputs
        if(myNav.fire()) mbedleds = 0x0F; //special all leds on case for fire (center button)
        //or use - if(myNav[4]==0) mbedleds = 0x0F; //can index a switch bit like this
        //wait(0.02);
        
        if (myNav[0] == 0) {
           a = 1;
        } else if (myNav[1] == 0) {
          a = 2;
        } else if (myNav[2] == 0) {
          a = 3;
        } else if (myNav[3] == 0) {
          a = 4;
        } else if (myNav[4] == 0) {
          a = 5;
        } else {
          a = 0;
        }
        }  
        if(a!=-1){
            //myled2 = 1;
            xbee1.putc(a); //XBee write
            //myled2 = 0;
            //myled = 1;
            wait(.2);
        }
    }
}