Diff: main.cpp

Revision:

0:cd1fe4f0ed39

Child:

1:93723df61425

Child:

2:a28cc676a454

Child:

4:b962f5a783a1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 08 20:01:10 2014 +0000
@@ -0,0 +1,218 @@
+#include "main.h"
+#include "mbed.h"
+#include "USBSerial.h"
+#include "Adafruit_SSD1306.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "SDFileSystem.h"
+
+//Virtual serial port over USB
+#ifdef PC_DEBUG
+USBSerial pc;
+#endif
+
+// Display
+#ifdef OLED_DEBUG
+SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
+Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0
+#endif
+
+// MPU
+MPU6050 mpu;
+InterruptIn dataReady(P0_15);
+
+// SD Card
+SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
+
+// LED for debug
+DigitalOut led(LED1);
+
+// Logging vars
+FILE *logFile;
+
+// Timer
+Timer totalTime;
+Timer captureTime;
+
+// Switch
+InterruptIn captureSwitch(P0_16);
+
+
+// State
+enum state {IDLE, CAPTURE};
+enum state State;
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+// Forward declarations
+void die(int flash_rate_s);
+bool log_open();
+void log_acceleration(VectorInt16 data);
+void log_close();
+void mpu_init();
+void get_data();
+
+volatile bool mpuInterrupt = false;
+void dataReadyISR() {
+    mpuInterrupt = true;
+}
+
+void captureSwitchISR() {
+    // used for debouncing
+    static int prev_time = 0;
+
+    if (totalTime.read_ms() - prev_time < 200)
+        return;
+        
+    State = (State == IDLE) ? CAPTURE : IDLE;
+    prev_time = totalTime.read_ms();
+}
+
+int main(void)
+{
+    totalTime.start();
+    
+    State = IDLE;
+    captureSwitch.mode(PullUp);
+    captureSwitch.rise(captureSwitchISR);   
+    
+    OLED_PRINTP("Waiting to capture.     ",0,0);
+    while (true) {
+        if (State == CAPTURE) {
+            PC_PRINTLN("Start capture button pressed!");
+            OLED_CLEAR();
+            OLED_PRINTPR("Starting capture.       ",0,0);
+            
+            // Start receiving data
+            PC_PRINTLN("Opening log file...");
+            if (!log_open()) {
+                OLED_CLEAR();
+                OLED_PRINTP("ERROR: SD (retry)", 0, 50);
+                State = IDLE;
+                continue;
+            }
+            
+            PC_PRINTLN("Initializing MPU...");
+            mpu_init();
+            
+            PC_PRINTLN("Starting capture...");
+            OLED_PRINTPR("Capturing data...       ",0,0);
+            captureTime.start();
+            while (State == CAPTURE)
+                get_data();
+            OLED_PRINTPR("Finished capture.",0,0);
+            
+            log_close();
+            captureTime.stop();
+        }
+    
+        PC_PRINTLN("Idling...");
+    }
+}
+
+/* Halts program and flashes LED at specified rate */
+void die(int flash_rate_s) {
+    while (1) {
+        led = 1;
+        wait(flash_rate_s/2);
+        led = 0;
+        wait(flash_rate_s/2);
+    }
+}
+
+/* Returns false on failure, true otherwise */
+bool log_open() {
+    logFile = fopen(LOG_FILE, "a");
+    if (logFile == NULL) {
+        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
+        return false;
+    }
+    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
+    return true;
+}
+
+void log_close() {
+    if (logFile != NULL)
+        fclose(logFile);
+}
+
+void mpu_init() {
+    PC_PRINTLN("Initializing MPU");
+    mpu.initialize();
+    devStatus = mpu.dmpInitialize();
+    
+    if (devStatus == 0) {
+        mpu.setDMPEnabled(true);
+        packetSize = mpu.dmpGetFIFOPacketSize();
+        
+        PC_PRINTLN("DMP Initialized successfully!");
+        dmpReady = true;
+        dataReady.rise(dataReadyISR);
+    } else { // ERROR
+        PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
+        die(DMP_ERROR_RATE);
+    }
+}
+
+/* Requires the log to be open and mpu to be initialized*/
+void get_data() {
+    //while (true) {
+        //if (!dmpReady) break;   // do nothing if dmp not ready
+        if (!dmpReady) return;
+        
+        while (!mpuInterrupt && fifoCount < packetSize);
+        
+        // Reset interrupt
+        mpuInterrupt = false;
+        mpuIntStatus = mpu.getIntStatus();
+
+        // get current FIFO count
+        fifoCount = mpu.getFIFOCount();
+    
+        // check for overflow (this should never happen unless our code is too inefficient)
+        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+            PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms());
+            // reset so we can continue cleanly
+            mpu.resetFIFO();
+            // otherwise, check for DMP data ready interrupt (this should happen frequently)
+        } else if (mpuIntStatus & 0x02) {
+            // Wait for a full packet - should be very short wait
+            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+    
+            // read a packet from FIFO
+            mpu.getFIFOBytes(fifoBuffer, packetSize);
+            fifoCount -= packetSize;
+            
+            // Get acceleration data
+            mpu.dmpGetAccel(&aa, fifoBuffer);
+            mpu.dmpGetQuaternion(&q, fifoBuffer);
+            mpu.dmpGetGravity(&gravity, &q);
+            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+            
+            PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z);
+            
+//            OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
+            
+            log_acceleration(aaWorld);
+        //}
+    }
+}
+
+void log_acceleration(VectorInt16 data) {
+    fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z);
+}
\ No newline at end of file