main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #define PI 3.14159
00004 #include "uLCD_4DGL.h"
00005 #include "mbed.h"
00006 
00007 Serial xbee1(p13, p14);
00008 DigitalOut rst1(p11);
00009 Serial pc(USBTX, USBRX);
00010 DigitalOut myled(LED1);
00011 DigitalOut myled2(LED2);
00012 DigitalOut myled3(LED3);
00013 BusOut mbedleds(LED1,LED2,LED3,LED4);
00014 //BusOut/In is faster than multiple DigitalOut/Ins
00015  
00016 class Nav_Switch
00017 {
00018 public:
00019     Nav_Switch(PinName up,PinName down,PinName left,PinName right,PinName fire);
00020     int read();
00021 //boolean functions to test each switch
00022     bool up();
00023     bool down();
00024     bool left();
00025     bool right();
00026     bool fire();
00027 //automatic read on RHS
00028     operator int ();
00029 //index to any switch array style
00030     bool operator[](int index) {
00031         return _pins[index];
00032     };
00033 private:
00034     BusIn _pins;
00035  
00036 };
00037 Nav_Switch::Nav_Switch (PinName up,PinName down,PinName left,PinName right,PinName fire):
00038     _pins(up, down, left, right, fire)
00039 {
00040     _pins.mode(PullUp); //needed if pullups not on board or a bare nav switch is used - delete otherwise
00041     wait(0.001); //delays just a bit for pullups to pull inputs high
00042 }
00043 inline bool Nav_Switch::up()
00044 {
00045     return !(_pins[0]);
00046 }
00047 inline bool Nav_Switch::down()
00048 {
00049     return !(_pins[1]);
00050 }
00051 inline bool Nav_Switch::left()
00052 {
00053     return !(_pins[2]);
00054 }
00055 inline bool Nav_Switch::right()
00056 {
00057     return !(_pins[3]);
00058 }
00059 inline bool Nav_Switch::fire()
00060 {
00061     return !(_pins[4]);
00062 }
00063 inline int Nav_Switch::read()
00064 {
00065     return _pins.read();
00066 }
00067 inline Nav_Switch::operator int ()
00068 {
00069     return _pins.read();
00070 }
00071  
00072 Nav_Switch myNav(p29,p26,p27,p25, p28); //pin order on Sparkfun breakout
00073 
00074 float y=0;
00075 float z=0;
00076 
00077 int main()
00078 {
00079      LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00080     IMU.begin();
00081     if (!IMU.begin()) {
00082         pc.printf("Failed to communicate with LSM9DS1.\n");
00083     }
00084     IMU.calibrate(1);
00085     
00086     wait(3);
00087     rst1 = 0;   //Set reset pin to 0
00088     myled = 1;
00089     //myled2= 0;
00090     wait_ms(1);
00091     rst1 = 1;   //Set reset pin to 1
00092     wait_ms(1);
00093    
00094     while(1) {
00095       int a;//getKeyNum();      
00096        int b;
00097        while(!IMU.accelAvailable());
00098         IMU.readAccel();
00099    
00100         //These conditional statements create a "dead-zone" for the wheel, to make it less
00101         //sensitive and more usable. Additionally, the IMU data will never exceed .9 to so
00102         //as to limit the maximum speed and steering for the robot.
00103         if(IMU.calcAccel(IMU.ay)<-.1) {
00104             b = 1;
00105             y=.9;
00106             a = 4;
00107             wait(.5);
00108         } else if (IMU.calcAccel(IMU.ay)>.1) {
00109             b = 1;
00110             y=-.9;
00111             a = 3;
00112             wait(.5);
00113         //} else if(IMU.calcAccel(IMU.ay)<=.1||IMU.calcAccel(IMU.ay)>=-.1) {
00114         //    y=IMU.calcAccel(IMU.ay);
00115             
00116         } else if(IMU.calcAccel(IMU.ax)>.1) {
00117             b = 1;
00118             z=.9;
00119             a = 2;
00120             wait(.5);
00121         } else if((IMU.calcAccel(IMU.ax)<-.1)) {
00122             b = 1;
00123             z=IMU.calcAccel(IMU.az);;
00124             a = 1;
00125             wait(.5);
00126         } else {
00127             a = 0;
00128             b = 0;
00129         } 
00130        
00131         if (b == 0) {
00132         //with pullups a button hit is a "0" - "~" inverts data to leds
00133         mbedleds = ~(myNav & 0x0F); //update leds with nav switch direction inputs
00134         if(myNav.fire()) mbedleds = 0x0F; //special all leds on case for fire (center button)
00135         //or use - if(myNav[4]==0) mbedleds = 0x0F; //can index a switch bit like this
00136         //wait(0.02);
00137         
00138         if (myNav[0] == 0) {
00139            a = 1;
00140         } else if (myNav[1] == 0) {
00141           a = 2;
00142         } else if (myNav[2] == 0) {
00143           a = 3;
00144         } else if (myNav[3] == 0) {
00145           a = 4;
00146         } else if (myNav[4] == 0) {
00147           a = 5;
00148         } else {
00149           a = 0;
00150         }
00151         }  
00152         if(a!=-1){
00153             //myled2 = 1;
00154             xbee1.putc(a); //XBee write
00155             //myled2 = 0;
00156             //myled = 1;
00157             wait(.2);
00158         }
00159     }
00160 }