Diff: main.cpp

Revision:

4:39e949908fc5

Parent:

3:990f8380da21

Child:

5:d9f8c2f63323

--- a/main.cpp	Tue Nov 24 20:30:53 2015 +0000
+++ b/main.cpp	Tue Dec 01 15:00:12 2015 +0000
@@ -2,19 +2,23 @@
 #include "C12832.h"
 #include "LM75B.h"
 #include "MMA7660.h"
-#include "FXOS8700CQ.h"
+#include "FXOS8700Q.h"
+#include "comms.h"
+#include "CalibrateMagneto.h"
 
 //OUTPUTS
 
-DigitalOut lr(PTB22), lg(PTE26), lb(PTB21);
-PwmOut xr(D5), xg(D9), xb(D8);
+DigitalOut lr(PTB22), lg(PTE26), lb(PTB21), xr(D5), xg(D9), xb(PTC12);
 C12832 lcd(D11, D13, D12, D7, D10);
 
 //INPUTS
 
 LM75B temp(D14, D15);
-MMA7660 accel(D14, D15);
-FXOS8700CQ magAccel(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
+MMA7660 sAcc(D14, D15);
+
+FXOS8700Q_acc bAcc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+FXOS8700Q_mag bMag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+CalibrateMagneto magCal;
 
 InterruptIn sw2_int(SW2), sw3_int(SW3), up_int(A2), down_int(A3), left_int(A4), right_int(A5), fire_int(D4);
 DigitalIn   sw2(SW2),     sw3(SW3),     up(A2),     down(A3),     left(A4),     right(A5),     fire(D4);
@@ -41,7 +45,7 @@
 
 //COMMS
 
-Serial pc(USBTX, USBRX);
+DataSerial pc(USBTX, USBRX);
 
 //INTERRUPT HANDLERS
 
@@ -66,6 +70,41 @@
 void fireDown(){ flags |= FIRE_DOWN; }
 void fireUp()  { flags |= FIRE_UP; }
 
+//PRETTY WAITING LEDS (Modified from Fred Barnes)
+
+Ticker waitTicker;
+int ctr;
+
+void waitTick() {
+    switch(ctr) {
+        case 0:
+            xr=xg=0; xb=1; break;
+        case 1:
+            xr=0; xg=xb=1; break;
+        case 2:
+            xr=xb=0; xg=1; break;
+        case 3:
+            xb=0; xr=xg=1; break;
+        case 4:
+            xg=xb=0; xr=1; break;
+        case 5:
+            xg=0; xr=xb=1; ctr=0; return;
+    }
+    ctr++;
+}
+
+void cleanup() {
+    lr = lg = lb = 1;
+    xr = xg = xb = 1.0;
+    temp.read();
+    lcd.cls();
+    
+    bAcc.enable();
+    bMag.enable();
+    
+    pc.sendChar('+');
+}
+
 /*
 Return the state of a requested pin
 */
@@ -81,7 +120,8 @@
         case 'f': val=fire; break;
         default: return;
     }
-    pc.printf(val?"t;":"f;");
+    char c = val?'t':'f';
+    pc.sendChar(c);
 }
 
 /*
@@ -94,57 +134,19 @@
         case '2': val=pot2; break;
         default: return;
     }
-    pc.printf("%6.4f;",val);
-}
-
-//BLOCK LEVEL READ & WRITE
-
-void sendData(void* data, int len) {
-    for(char* c = (char*)data;len--;)
-        pc.putc(*(c++));
-}
-
-void readData(void* data, int len) {
-    char* c = (char*)data;
-    while(len>0) {
-        *c = pc.getc();
-        c++;
-        len--;
-    }
+    
+    pc.sendFloat(val);
 }
 
-//PRETTY WAITING LEDS (Modified from Fred Barnes)
-
-Ticker waitTicker;
-int ctr;
-
-uint8_t brilvl(int offset) {
-    if(offset>311) offset-=312;
-    if (offset < 52) return (uint8_t)(offset * 5);
-    if (offset < 156) return 255;
-    if (offset < 208) return (uint8_t)((207 - offset) * 5);
-    return 0;
-}
-
-void waitTick() {
-    ctr++;//
-//    if (ctr > 311) {
-//        ctr -= 312;
-//    }
-//    xr = brilvl(  ctr  )/255.0;
-//    xg = brilvl(ctr+104)/255.0;
-//    xb = brilvl(ctr+208)/255.0;
-    xr = 1;
-    xg = 1;
-    xb = 0;
-}
-
-void cleanup() {
-    lr = lg = lb = 1;
-    xr = xg = xb = 1;
-    temp.read();
-    lcd.cls();
-    pc.putc('+');
+float getHeading() {
+    MotionSensorDataUnits mag;
+    bMag.getAxis(mag);
+    float cal[3];
+    cal[0]=mag.x;
+    cal[1]=mag.y;
+    cal[2]=mag.z;
+    magCal.run(cal,cal);
+    return atan2(-cal[1],cal[0])-0.4333f;
 }
 
 int main() {  
@@ -152,6 +154,7 @@
     pc.baud(9600);
     
     lr = lg = lb = 1;
+    xr = xg = xb = 1.0;
     
     sw2_int.mode (PullUp);
     sw2_int.fall(&sw2Down);
@@ -162,98 +165,91 @@
     sw3_int.rise(&sw3Up);
     
     up_int.mode (PullUp);
-    up_int.fall(&upDown);
-    up_int.rise(&upUp);
-    up_int.disable_irq();
+    up_int.rise(&upDown);
+    up_int.fall(&upUp);
     
     down_int.mode (PullUp);
-    down_int.fall(&downDown);
-    down_int.rise(&downUp);
+    down_int.rise(&downDown);
+    down_int.fall(&downUp);
     
     left_int.mode (PullUp);
-    left_int.fall(&leftDown);
-    left_int.rise(&leftUp);
+    left_int.rise(&leftDown);
+    left_int.fall(&leftUp);
     
     right_int.mode (PullUp);
-    right_int.fall(&rightDown);
-    right_int.rise(&rightUp);
+    right_int.rise(&rightDown);
+    right_int.fall(&rightUp);
     
     fire_int.mode (PullUp);
-    fire_int.fall(&fireDown);
-    fire_int.rise(&fireUp);
+    fire_int.rise(&fireDown);
+    fire_int.fall(&fireUp);
     
     //Pretty wait
     ctr=0;
     lcd.locate(0,0);
     lcd.printf("Awaiting connection...");
-    waitTicker.attach(&waitTick,0.02);
-    while(pc.getc()!='-');
+    waitTicker.attach(&waitTick,0.5);
+    while((!pc.readable()) && (pc.getc()!='-')) sleep();
     waitTicker.detach();
     cleanup();
     
-    union {
-        float f;
-        int i;
-        char c[4];
-    } val;
-    
     for(;;) {
         //Check if any interrupts fired
         if(flags&SW2_UP) {
             flags&=!SW2_UP;
-            pc.printf("!2u");
+            pc.sendSpecialCommand('2','u');
         }
         if(flags&SW2_DOWN) {
             flags&=!SW2_DOWN;
-            pc.printf("!2d");
+            pc.sendSpecialCommand('2','d');
         }
         if(flags&SW3_UP) {
             flags&=!SW3_UP;
-            pc.printf("!3u");
+            pc.sendSpecialCommand('3','u');
         }
         if(flags&SW3_DOWN) {
             flags&=!SW3_DOWN;
-            pc.printf("!3d");
+            pc.sendSpecialCommand('3','d');
         }
         if(flags&UP_UP) {
             flags&=!UP_UP;
-            pc.printf("!uu");
+            pc.sendSpecialCommand('u','u');
         }
         if(flags&UP_DOWN) {
             flags&=!UP_DOWN;
-            pc.printf("!ud");
+            pc.sendSpecialCommand('u','d');
         }
         if(flags&DOWN_UP) {
             flags&=!DOWN_UP;
-            pc.printf("!du");
+            pc.sendSpecialCommand('d','u');
         }
         if(flags&DOWN_DOWN) {
             flags&=!DOWN_DOWN;
-            pc.printf("!dd");
+            pc.sendSpecialCommand('d','d');
         }
         if(flags&LEFT_UP) {
             flags&=!LEFT_UP;
-            pc.printf("!lu");
+            pc.sendSpecialCommand('l','u');
         }
         if(flags&LEFT_DOWN) {
             flags&=!LEFT_DOWN;
-            pc.printf("!ld");
+            pc.sendSpecialCommand('l','d');
         }
         if(flags&RIGHT_UP) {
             flags&=!RIGHT_UP;
-            pc.printf("!ru");
+            pc.sendSpecialCommand('r','u');
         }
         if(flags&RIGHT_DOWN) {
             flags&=!RIGHT_DOWN;
-            pc.printf("!rd");
+            pc.sendSpecialCommand('r','d');
         }
         if(flags&FIRE_UP) {
             flags&=!FIRE_UP;
-            pc.printf("!fu");
+            pc.sendSpecialCommand('f','u');
         }
         if(flags&FIRE_DOWN) {
             flags&=!FIRE_DOWN;
-            pc.printf("!fd");
+            pc.sendSpecialCommand('f','d');
         }
         
         //Check for host requests
@@ -270,49 +266,72 @@
                 }
                 case 't': { //read temp
                     float t = temp.read();
-                    sendData(&t,4);
+                    pc.sendFloat(t);
                     break;
                 }
                 case 'a': {//read shield accel
                     float x, y, z;
-                    x = accel.x();
-                    y = accel.y();
-                    z = accel.z();
-                    sendData(&x,4);
-                    sendData(&y,4);
-                    sendData(&z,4);
+                    x = sAcc.x();
+                    y = sAcc.y();
+                    z = sAcc.z();
+                    pc.sendFloat(x);
+                    pc.sendFloat(y);
+                    pc.sendFloat(z);
                     break;
                 }
                 case 'l': { //shield led
-                    float r, g, b;
-                    readData(&r,4);
-                    readData(&g,4);
-                    readData(&b,4);
-                    xr = 1.0f-r; xg = 1.0f-g; xb = 1.0f-b;
+                    char raw = pc.getc();
+                    char r = ((raw>>2)&1)^1;
+                    char g = ((raw>>1)&1)^1;
+                    char b = (raw&1)^1;
+                    xr = r; xg = g; xb = b;
+                    break;
+                }
+                case 'o': { //read shield accel orientation
+                    MMA7660::Orientation o = sAcc.getOrientation();
+                    pc.sendChar((char)o);
+                    break;
+                }
+                case 's': { //read shield accel orientation
+                    MMA7660::Orientation o = sAcc.getSide();
+                    pc.sendChar((char)o);
+                    break;
+                }
+                case 'h': {
+                    float f = getHeading();
+                    pc.sendFloat(f);
                     break;
                 }
                 case 'L': { // board led
                     char raw = pc.getc();
-                    char r = ((raw>>2)&1)?0:1;
-                    char g = ((raw>>1)&1)?0:1;
-                    char b = (raw&1)?0:1;
+                    char r = ((raw>>2)&1)^1;
+                    char g = ((raw>>1)&1)^1;
+                    char b = (raw&1)^1;
                     lr = r; lg = g; lb = b;
                     break;
                 }
                 case 'M': { //read board magno
-                    SRAWDATA m, a;
-                    magAccel.get_data(&a,&m);
-                    sendData(&m.x,2);
-                    sendData(&m.y,2);
-                    sendData(&m.z,2);
+//                    float f;
+//                    bMag.getX(&f); pc.sendFloat(f);
+//                    bMag.getY(&f); pc.sendFloat(f);
+//                    bMag.getZ(&f); pc.sendFloat(f);
+                    MotionSensorDataUnits mag;
+                    bMag.getAxis(mag);
+                    float cal[3];
+                    cal[0]=mag.x;
+                    cal[1]=mag.y;
+                    cal[2]=mag.z;
+                    magCal.run(cal,cal);
+                    pc.sendFloat(cal[0]);
+                    pc.sendFloat(cal[1]);
+                    pc.sendFloat(cal[2]);
                     break;
                 }
                 case 'A': { //read board accel
-                    SRAWDATA m, a;
-                    magAccel.get_data(&a,&m);
-                    sendData(&a.x,2);
-                    sendData(&a.y,2);
-                    sendData(&a.z,2);
+                    float f;
+                    bAcc.getX(&f); pc.sendFloat(f);
+                    bAcc.getY(&f); pc.sendFloat(f);
+                    bAcc.getZ(&f); pc.sendFloat(f);
                     break;
                 }
                 case 'P': { //set lcd position
@@ -332,6 +351,7 @@
                     }
                     *j=0;
                     lcd.printf(buff);
+                    lcd.copy_to_lcd();
                     break;
                 }
                 case 'D': { //set lcd pixel
@@ -339,6 +359,7 @@
                     char y = pc.getc();
                     char c = pc.getc();
                     lcd.pixel(x,y,c);
+                    lcd.copy_to_lcd();
                     break;
                 }
                 case 'C': { //clear lcd
@@ -352,4 +373,4 @@
             }
         }
     }
-}
+}
\ No newline at end of file