Diff: main.cpp

Revision:

1:fdfa313b9cc3

Parent:

0:a260d84e07fc

Child:

2:d8b182fbe018

--- a/main.cpp	Sun Oct 16 05:37:27 2011 +0000
+++ b/main.cpp	Mon Oct 17 15:19:01 2011 +0000
@@ -1,14 +1,12 @@
-/* 
+/*
  * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current
  * Updated to use interrupts - this will help when we intergrate this code into AVNavControl
  * 9dof razor from sparkfun
  */
- 
+
 #define GYRO_SCALE 14.375  // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf 
 
 #include "mbed.h"
-#include <string>
-#include <sstream>
 
 Serial IMU(p9, p10);   // tx, rx
 Serial PC(USBTX, USBRX);
@@ -20,53 +18,106 @@
 
 bool IMUreadable = false;
 bool PCreadable = false;
-bool parseNow = false;
-
-bool debugMode = false;
-
-string buffer;
 
 void readIMU();
 void readPC();
+inline void makeCorrect(short* i);
 
-void parse(string);
+short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
 
-int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
+// Expected format: {$, accX, accX, accY, accY, accZ, accZ, gyrX, gyrX, gyrY, gyrY, gyrZ, gyrZ, magX, magX, magY, magY, magZ, magZ, #, \r}
+int i_IMU;
+char bufIMU[21];
 
 int main() {
+    // Set up the connection. Read up about parity and stop bits if this is confusing.
     IMU.format(8, Serial::None, 1);
     PC.format(8, Serial::None, 1);
     IMU.baud(57600);
     PC.baud(115200);
-    
-    PC.printf("----------------------------\n\r");
-    PC.printf("debug: device reset\n\r");
-    PC.printf("debug: gotta have my bowl; gotta have serial. serial is up.\n\r");
-    
+
+    for (int i = 0; i < 80; i++) {
+        PC.putc('-');
+    }
+    PC.printf("\n\r");
+
     PC.attach(&readPC);
     IMU.attach(&readIMU);
-    
-    //__disable_irq();
-    PC.printf("debug: attached interrupts.\n\r");
-        
-    if (!debugMode) IMU.putc('4');  //tell the imu to start sending data, if it's not sending data already
-    PC.printf("debug: told the IMU to start sending data\n\r");
-    
-    //The main loop 
-    while (1) {
+
+    IMU.putc('6');  //tell the IMU to start sending data in binary mode, if it's not sending data already
+
+    //The main loop
+    while (true) {
         __enable_irq();
         if (IMUreadable) {
             myled2 = 1;
-            if (debugMode){
-                while (IMU.readable()) PC.putc(IMU.getc());
+
+            while (IMU.readable()) {
+                // This snippet of code should be run whenever a character can be received from the IMU.
+
+                char data = IMU.getc();
+
+                // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there.
+                if (data == '$') {
+                    i_IMU = 0;
+                    printf("new data\n\r");
+                }
+                // Something went wrong.
+                else if (i_IMU > 21) {
+                    printf("\t\t\tIMU error.\n\r");
+                    i_IMU = 21;
+                }
+
+                // End of the set of data. Parse the buffer
+                else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
+                    printf("Parsing\n\r");
+                    
+                    //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
+                    //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
+                    //left-shifting the first character by 8 bits and ORing it with the second character.
+                    accX = (bufIMU[1]<<8 | bufIMU[2]);
+                    accY = (bufIMU[3]<<8 | bufIMU[4]);
+                    accZ = (bufIMU[5]<<8 | bufIMU[6]);
+
+                    gyrX = (bufIMU[7]<<8 | bufIMU[8]);
+                    gyrY = (bufIMU[9]<<8 | bufIMU[10]);
+                    gyrZ = (bufIMU[11]<<8 | bufIMU[12]);
+
+                    magX = (bufIMU[13]<<8 | bufIMU[14]);
+                    magY = (bufIMU[15]<<8 | bufIMU[16]);
+                    magZ = (bufIMU[17]<<8 | bufIMU[18]);
+                    
+                    makeCorrect(&accX);
+                    makeCorrect(&accY);
+                    makeCorrect(&accZ);
+
+                    makeCorrect(&gyrX);
+                    makeCorrect(&gyrY);
+                    makeCorrect(&gyrZ);
+
+                    makeCorrect(&magX);
+                    makeCorrect(&magY);
+                    makeCorrect(&magZ);
+
+
+                    PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
+
+                    //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
+
+                    /*
+                    for (int i = 0; i < 21; i++) {
+                        PC.printf("%d  ", bufIMU[i]);
+                    }
+                    PC.printf("\n\r");
+                    */
+                    //newIMUData = 1; // Update the flag
+                }
+
+
+                bufIMU[i_IMU] = data;
+                i_IMU++;
+                //parseNow = (buffer.at(buffer.length() - 1) == '#');
             }
-            else {
-                while (IMU.readable()){
-                    buffer.push_back(IMU.getc());
-                    parseNow = (buffer.at(buffer.length() - 1) == '#');
-                }
-            }
-            
             IMUreadable = false;
             myled2 = 0;
         }
@@ -76,36 +127,33 @@
             PCreadable = false;
             myled1 = 0;
         }
-        if (parseNow) {
+        /*if (parseNow) {
             parse(buffer);
             buffer.clear();
             parseNow = false;
-        }
+        }*/
     }
 }
 
 //Interrupt called when there is a character to be read from the PC
 //To avoid a livelock, we disable interrupts at the end of the interrupt.
 //Then, in the main loop, we read everything from the buffer
-void readPC(){
+void readPC() {
     PCreadable = true;
     __disable_irq();
 }
 
 //Interrupt called when there is a character to be read from the IMU
-void readIMU(){   
+void readIMU() {
     IMUreadable = true;
     __disable_irq();
 }
 
-//Checks data integrity, then stores the IMU values into variables
-void parse(string buf){
-    if (buf.find('$') != buf.npos && buf.find('#') != buf.npos){
-        for (int i = 0; i < buf.length(); i++) if (buf[i] == '$' || buf[i] == ',' || buf[i] == '#') buf[i] = ' ';
-        stringstream ss(buf);
-        ss >> accX >> accY >> accZ >> gyrX >> gyrY >> gyrZ >> magX >> magY >> magZ;
 
-        PC.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
-    }
-    else PC.printf("Bad parse!\n\r");
-}
+//The bitshift performed always creates a positive integer. Sometimes, the IMU
+//values are negative. This fixes that - it centers the value around 512. That is,
+//512 is 0 for the IMU variables. Values above it are positive, and values below it are negative.
+inline void makeCorrect (short* i) {
+    if ((*i)>>15) *i = 512 - (~(*i));
+    else *i = 512 + *i;
+}
\ No newline at end of file