Diff: main.cpp

Revision:

0:a260d84e07fc

Child:

1:fdfa313b9cc3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 16 05:37:27 2011 +0000
@@ -0,0 +1,111 @@
+/* 
+ * 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);
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+bool IMUreadable = false;
+bool PCreadable = false;
+bool parseNow = false;
+
+bool debugMode = false;
+
+string buffer;
+
+void readIMU();
+void readPC();
+
+void parse(string);
+
+int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
+
+int main() {
+    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");
+    
+    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) {
+        __enable_irq();
+        if (IMUreadable) {
+            myled2 = 1;
+            if (debugMode){
+                while (IMU.readable()) PC.putc(IMU.getc());
+            }
+            else {
+                while (IMU.readable()){
+                    buffer.push_back(IMU.getc());
+                    parseNow = (buffer.at(buffer.length() - 1) == '#');
+                }
+            }
+            
+            IMUreadable = false;
+            myled2 = 0;
+        }
+        if (PCreadable) {
+            myled1 = 1;
+            while (PC.readable()) IMU.putc(PC.getc());
+            PCreadable = false;
+            myled1 = 0;
+        }
+        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(){
+    PCreadable = true;
+    __disable_irq();
+}
+
+//Interrupt called when there is a character to be read from the IMU
+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");
+}