Diff: main.cpp

Revision:

5:d9f8c2f63323

Parent:

4:39e949908fc5

Child:

6:adf2837c1e7f

--- a/main.cpp	Tue Dec 01 15:00:12 2015 +0000
+++ b/main.cpp	Fri Dec 04 14:42:01 2015 +0000
@@ -4,12 +4,14 @@
 #include "MMA7660.h"
 #include "FXOS8700Q.h"
 #include "comms.h"
-#include "CalibrateMagneto.h"
+#include "main.h"
+# define M_PI           3.14159265358979323846  /* pi */
 
 //OUTPUTS
 
 DigitalOut lr(PTB22), lg(PTE26), lb(PTB21), xr(D5), xg(D9), xb(PTC12);
 C12832 lcd(D11, D13, D12, D7, D10);
+PwmOut spkr(D6);
 
 //INPUTS
 
@@ -18,7 +20,6 @@
 
 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);
@@ -47,6 +48,8 @@
 
 DataSerial pc(USBTX, USBRX);
 
+
+
 //INTERRUPT HANDLERS
 
 void sw2Down(){ flags |= SW2_DOWN; }
@@ -88,29 +91,19 @@
         case 4:
             xg=xb=0; xr=1; break;
         case 5:
-            xg=0; xr=xb=1; ctr=0; return;
+            lr=xg=lb=0; xr=lg=xb=1;
+            ctr=0; return;
     }
+    lr=!xr; lg=!xg; lb=!xb;
     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
 */
-void readDigitalIn() {
+void read_digital() {
     bool val;
-    switch(pc.getc()) {
+    switch(pc.readChar()) {
         case '2': val=sw2; break;
         case '3': val=sw3; break;
         case 'u': val=up; break;
@@ -127,34 +120,138 @@
 /*
 Return the state of a requested potentiometer
 */
-void readPot() {
+void read_pot() {
     float val;
-    switch(pc.getc()) {
+    switch(pc.readChar()) {
         case '1': val=pot1; break;
         case '2': val=pot2; break;
         default: return;
     }
-    
     pc.sendFloat(val);
 }
+void read_temp() { //read temp
+    float t = temp.read();
+    pc.sendFloat(t);
+}
 
-float getHeading() {
+void read_shield_accel() {//read shield accel
+    float x, y, z;
+    x = sAcc.x();
+    y = sAcc.y();
+    z = sAcc.z();
+    pc.sendFloat(x);
+    pc.sendFloat(y);
+    pc.sendFloat(z);
+}
+
+void set_shield_led() { //shield led
+    char raw = pc.readChar();
+    char r = ((raw>>2)&1)^1;
+    char g = ((raw>>1)&1)^1;
+    char b = (raw&1)^1;
+    xr = r; xg = g; xb = b;
+}
+
+void get_orientation() { //read shield accel orientation
+    MMA7660::Orientation o = sAcc.getOrientation();
+    pc.sendChar((char)o);
+}
+
+void get_side() { //read shield accel orientation
+    MMA7660::Orientation o = sAcc.getSide();
+    pc.sendChar((char)o);
+}
+
+void get_heading() {
     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;
+    float x = mag.x;
+    float y = mag.y;
+    float heading;
+    if( y == 0 ) {
+        heading = (x < 0 ? 0.0f : 180.0f);
+    } else  {
+        heading = (y > 0 ? 270.0f : 90.0f) - (atan((x/y))*(180.0f/(float)M_PI));
+    }
+    pc.sendFloat(heading);
+}
+
+void set_board_led() { // board led
+    char raw = pc.readChar();
+    char r = ((raw>>2)&1)^1;
+    char g = ((raw>>1)&1)^1;
+    char b = (raw&1)^1;
+    lr = r; lg = g; lb = b;
+}
+
+void read_board_magno() { //read board magno
+    MotionSensorDataUnits mag;
+    bMag.getAxis(mag);
+    pc.sendFloat(mag.x);
+    pc.sendFloat(mag.y);
+    pc.sendFloat(mag.z);
+}
+
+void read_board_accel() { //read board accel
+    float f;
+    bAcc.getX(&f); pc.sendFloat(f);
+    bAcc.getY(&f); pc.sendFloat(f);
+    bAcc.getZ(&f); pc.sendFloat(f);
+}
+
+void set_lcd_position() { //set lcd position
+    unsigned char xy[2];
+    pc.readData(xy,2);
+    lcd.locate(xy[0],xy[1]);
+}
+
+void print_text() { //print lcd text
+    char buff[256];
+    char len = pc.readString(buff);
+    buff[len]=0;
+    lcd.printf(buff);
+    lcd.copy_to_lcd();
+}
+
+void set_lcd_pixel() { //set lcd pixel
+    char xyc[3];
+    pc.readData(xyc, 3);
+    lcd.pixel(xyc[0],xyc[1],xyc[2]);
+    lcd.copy_to_lcd();
+}
+
+void set_piezo() { // set piezo buzzer
+    float vol = pc.readFloat();
+    float period = 1.0f/pc.readFloat();
+    spkr=vol*0.5f; //convert volume to duty cycle
+    spkr.period(period);
+}
+
+void clear_lcd() { //clear lcd
+    lcd.cls();
+}
+
+void cleanup() {
+    lr = lg = lb = 1;
+    xr = xg = xb = 1;
+    lcd.cls();
+    
+    pc.sendChar(RECONNECT);
+}
+
+void reconnect() { //reconnection
+    cleanup();
 }
 
 int main() {  
     //SETUP
-    pc.baud(9600);
+    pc.baud(115200);
     
     lr = lg = lb = 1;
-    xr = xg = xb = 1.0;
+    xr = xg = xb = 1;
+    
+    bAcc.enable();
+    bMag.enable();
     
     sw2_int.mode (PullUp);
     sw2_int.fall(&sw2Down);
@@ -189,7 +286,7 @@
     lcd.locate(0,0);
     lcd.printf("Awaiting connection...");
     waitTicker.attach(&waitTick,0.5);
-    while((!pc.readable()) && (pc.getc()!='-')) sleep();
+    while((!pc.readable()) && (pc.readChar()!=RECONNECT)) sleep();
     waitTicker.detach();
     cleanup();
     
@@ -254,122 +351,9 @@
         
         //Check for host requests
         if(pc.readable()) {
-            char cmd = pc.getc();
-            switch(cmd) {
-                case 'R': { //read digital input
-                    readDigitalIn();
-                    break;
-                }
-                case 'p': {
-                    readPot();
-                    break;
-                }
-                case 't': { //read temp
-                    float t = temp.read();
-                    pc.sendFloat(t);
-                    break;
-                }
-                case 'a': {//read shield accel
-                    float x, y, z;
-                    x = sAcc.x();
-                    y = sAcc.y();
-                    z = sAcc.z();
-                    pc.sendFloat(x);
-                    pc.sendFloat(y);
-                    pc.sendFloat(z);
-                    break;
-                }
-                case 'l': { //shield led
-                    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)^1;
-                    char g = ((raw>>1)&1)^1;
-                    char b = (raw&1)^1;
-                    lr = r; lg = g; lb = b;
-                    break;
-                }
-                case 'M': { //read board magno
-//                    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
-                    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
-                    unsigned char x, y;
-                    x = pc.getc();
-                    y = pc.getc();
-                    lcd.locate(x,y);
-                    break;
-                }
-                case 'S': { //print lcd text
-                    char buff[256];
-                    unsigned char len = pc.getc();
-                    char * j= buff;
-                    while(len>0) {
-                        *j++=pc.getc();
-                        len--;
-                    }
-                    *j=0;
-                    lcd.printf(buff);
-                    lcd.copy_to_lcd();
-                    break;
-                }
-                case 'D': { //set lcd pixel
-                    char x = pc.getc();
-                    char y = pc.getc();
-                    char c = pc.getc();
-                    lcd.pixel(x,y,c);
-                    lcd.copy_to_lcd();
-                    break;
-                }
-                case 'C': { //clear lcd
-                    lcd.cls();
-                    break;
-                }
-                case '-': { //reconnection
-                    cleanup(); break;
-                }
-                default: break;
+            unsigned char cmd = pc.readChar();
+            if(cmd<COMMAND_LENGTH) {
+                commandFunctions[cmd]();
             }
         }
     }