Revision 1:036b11214212, committed 2016-11-04

Comitter:

etorres31

Date:

Fri Nov 04 16:28:04 2016 +0000

Parent:

0:e693d5bf0a25

Commit message:

Xbee controller code;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/4DGL-uLCD-SE.lib	Fri Nov 04 16:28:04 2016 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/4180_1/code/4DGL-uLCD-SE/#e39a44de229a

--- a/main.cpp	Wed Feb 03 18:47:07 2016 +0000
+++ b/main.cpp	Fri Nov 04 16:28:04 2016 +0000
@@ -1,79 +1,160 @@
 #include "mbed.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.
+#include "uLCD_4DGL.h"
+#include "mbed.h"
 
+Serial xbee1(p13, p14);
+DigitalOut rst1(p11);
+Serial pc(USBTX, USBRX);
 DigitalOut myled(LED1);
-Serial pc(USBTX, USBRX);
-// Calculate pitch, roll, and heading.
-// Pitch/roll calculations taken from this app note:
-// http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
-// Heading calculations taken from this app note:
-// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+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)
 {
-    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;
+    _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
 
-
-    // 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);
-}
-
-
-
+float y=0;
+float z=0;
 
 int main()
 {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
     IMU.begin();
     if (!IMU.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
     }
     IMU.calibrate(1);
-    IMU.calibrateMag(0);
+    
+    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) {
-        while(!IMU.tempAvailable());
-        IMU.readTemp();
-        while(!IMU.magAvailable(X_AXIS));
-        IMU.readMag();
-        while(!IMU.accelAvailable());
+      int a;//getKeyNum();      
+       int b;
+       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));
-        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));
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
+   
+        //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);
+        }
     }
-}
-
+}
\ No newline at end of file