Revision 8:8430a5c0914c, committed 2014-05-20

Comitter:

ellingjp

Date:

Tue May 20 00:43:55 2014 +0000

Parent:

7:33a74adff0ff

Child:

9:a711b5b34d73

Commit message:

Working logging and split time detection

Changed in this revision

--- a/MPU6050-DMP.lib	Sat May 17 01:20:31 2014 +0000
+++ b/MPU6050-DMP.lib	Tue May 20 00:43:55 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/paulbartell/code/MPU6050-DMP/#e61356440739
+http://mbed.org/users/paulbartell/code/MPU6050-DMP/#322d2a246ba3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PinDetect.lib	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemTime.cpp	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,18 @@
+#include "mbed.h"
+#include "SystemTime.h"
+
+bool SystemTime::initialized = false;
+Timer SystemTime::sysTime;
+
+void SystemTime::start() 
+{
+    if (!initialized) {
+        sysTime.start();
+        initialized = true;
+    }
+}
+
+int SystemTime::read_ms()
+{
+    return sysTime.read_ms();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemTime.h	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,16 @@
+#ifndef _SYSTEMTIME_H
+#define _SYSTEMTIME_H
+
+#include "mbed.h"
+
+class SystemTime {
+    public:
+        static void start();
+        static int read_ms();
+    
+    private:
+        static bool initialized;
+        static Timer sysTime;
+};
+
+#endif // _SYSTEMTIME_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/log_data.cpp	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,48 @@
+#include "mbed.h"
+#include "log_data.h"
+#include "SDFileSystem.h"
+#include "helper_3dmath.h"
+#include "debug.h"
+
+// SD Card
+SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
+
+// Logging vars
+FILE *logFile;
+
+/* Returns true if logging was successfully initialized, false otherwise */
+bool log_init() {
+    logFile = fopen(LOG_FILE, "a");
+    if (logFile == NULL) {
+        PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
+        DIE(SD_ERROR_RATE);
+        return false;
+    }
+    fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
+    return true;
+}
+
+/* Returns true if data was successfully logged, false otherwise 
+   Used for logging acceleration data */
+bool log_data(VectorInt16 *data) {
+    if (logFile != NULL) {
+        fprintf(logFile, "%d, %d, %d\n", data->x, data->y, data->z);
+        return true;
+    }
+    
+    return false;
+}
+
+/* Returns true if data was successfully logged, false otherwise 
+   Used for logging split times */
+bool log_data(uint16_t data) {
+    
+}
+
+/* Returns true if logging was successfully closed, false otherwise */
+bool log_close() {
+    if (logFile != NULL)
+        return fclose(logFile) == 0;
+    
+    return false;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/log_data.h	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,11 @@
+#ifndef _LOG_FILE_H
+#define _LOG_FILE_H
+
+#include "helper_3dmath.h"
+#define LOG_FILE "/sd/data.log"
+
+bool log_init();
+bool log_data(VectorInt16 *data);
+bool log_close();
+
+#endif // _LOG_FILE_H
\ No newline at end of file

--- a/main.cpp	Sat May 17 01:20:31 2014 +0000
+++ b/main.cpp	Tue May 20 00:43:55 2014 +0000
@@ -1,87 +1,105 @@
 #include "main.h"
 #include "mbed.h"
+#include "PinDetect.h"
 #include "USBSerial.h"
 #include "Adafruit_SSD1306.h"
 #include "SDFileSystem.h"
 #include "receive_data.h"
+#include "log_data.h"
+#include "process_data.h"
 #include "debug.h"
+#include "SystemTime.h"
 
+// Capture button stuff
+#define PIN_DETECT_SAMPLE_PERIOD_uS 20000   // 20 ms sample period
+#define SYNC_HELD_SAMPLES   (SYNC_HOLD_TIME_MS * 1000 / PIN_DETECT_SAMPLE_PERIOD_uS)
+
+#ifdef USE_OLED
 // Display
-#ifdef OLED_DEBUG
 SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
 Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // DC, RST, CS
 #endif
 
-// SD Card
-SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
-
-// Logging vars
-FILE *logFile;
-
-// Timer
-Timer totalTime;
-Timer captureTime;
-
-// Switch
-InterruptIn captureSwitch(P0_16);
-
+// Mode button
+PinDetect captureButton(P0_16, PullUp);
 
 // State
-enum state {IDLE, CAPTURE};
+enum state {IDLE, CAPTURE, SYNC};
 enum state State;
 
-void captureSwitchISR() {
-    // used for debouncing
-    static int prev_time = 0;
-    int curr_time = totalTime.read_ms();
+// This is used to ensure the rising edge after a hold is ignored
+bool ignore_edge = false;
+void buttonHeld() {
+    if (State == IDLE)
+        State = SYNC;
+    else
+        State = IDLE;
     
-    // Only change state after an amount of time
-    // Note: abs value is necessary in case of 
-    //   overflows
-    if (abs(curr_time - prev_time) > 200)
-        State = (State == IDLE) ? CAPTURE : IDLE;
-        
-    prev_time = curr_time;
+    // Button was held down, so don't count the next assert
+    ignore_edge = true;
+}
+
+void buttonPressed() {
+    // Don't ignore subsequent edges
+    if (ignore_edge) {
+        ignore_edge = false;
+        return;
+    }
+    
+    if (State == IDLE)
+        State = CAPTURE;
+    else if (State == CAPTURE)
+        State = IDLE;
 }
 
 int main(void)
 {
-    totalTime.start();
+    SystemTime::start();
     
     State = IDLE;
-    captureSwitch.mode(PullUp);
-    captureSwitch.rise(captureSwitchISR);   
     
+    // After button is held, the next rising edge will call buttonPressed.
+    //   Because of this, inside the callbacks need to ensure that edge
+    //   does not do anything unintended.
+    captureButton.attach_deasserted_held(buttonHeld);
+    captureButton.attach_asserted(buttonPressed);
+    captureButton.setSampleFrequency();
+    captureButton.setSamplesTillHeld(SYNC_HELD_SAMPLES);
+
+    
+    VectorInt16 *data;
     while (true) {
         if (State == IDLE){
-            PC_PRINT("Idling\r");
+            OLED_CLEAR();
+            OLED_PRINTP("Idling...", 0, 0);
+            PC_PRINTLN("Idling...");
         } else if (State == CAPTURE) {
-            receive_init();
+            OLED_PRINTP("Starting capture...", 0, 0);
+            OLED_PRINTP("Init SD card...", 0, 10);
+            log_init();
+            OLED_PRINTP("Init peak detect...", 0, 10);
+            process_init();
+            OLED_PRINTP("Init data receipt...", 0, 10);
+            receive_init();           
+            OLED_CLEAR();
+            OLED_PRINTP("Capturing data...", 0, 0);
+            
+            
             while (State == CAPTURE) {
-                __disable_irq();
-                receive_data();
-                __enable_irq();
+                data = receive_data();
+                log_data(data);
+                int split = process_data(data->x);
+                if (split != UINT16_MAX) {
+                    PC_PRINTLNF("Split time: %d", split);
+                    OLED_PRINTPF("Split: %d", split, 0, 40);
+                }
             }
-            // data = receive_data();
-            // log_data(data);
-            // split = get_split(data);
-            // display_split;
-        } 
-        // else if (State == SYNC) {
-        // begin_sync();
-        // }
+            
+            receive_close();
+            process_close();
+            log_close();
+        } else if (State == SYNC) {
+            OLED_PRINTP("Ready to sync...", 0, 0);
+        }
     }
 }
-
-
-
-/* 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;
-//}
\ No newline at end of file

--- a/main.h	Sat May 17 01:20:31 2014 +0000
+++ b/main.h	Tue May 20 00:43:55 2014 +0000
@@ -1,7 +1,8 @@
-#define LOG_FILE "/sd/data.log"
+#define SYNC_HOLD_TIME_MS 1000
+#define DEBOUNCE_TIME_MS 200
 
-// #define OLED_DEBUG
-#ifdef OLED_DEBUG
+#define USE_OLED
+#ifdef USE_OLED
     #define OLED_SETCURS(xpos,ypos) oled.setCursor(xpos,ypos);
     #define OLED_CLEAR() oled.clearDisplay();
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/process_data.cpp	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,128 @@
+#include "mbed.h"
+#include "process_data.h"
+#include "SystemTime.h"
+#include "debug.h"
+
+int window = 0;
+int16_t *history;
+int16_t prev_data = 0;
+
+Timer split_timer;
+
+/* Initializes the moving average with a window size */
+bool process_init()
+{
+    window = WINDOW_SIZE;
+    history = new int16_t[WINDOW_SIZE](); // initializes to 0
+
+    split_timer.start();
+    return true;
+}
+
+/* Will break for window size > 2^8
+ * Returns a moving average for data
+ * Returns INT16_MAX until it has received a window
+ * size worth of data. These values should be ignored. */
+int16_t mov_avg(int16_t data)
+{
+    static uint8_t front = 0, end_ = 0, count = 0;
+    static int32_t total;
+
+    if (count < window-1) {
+        history[end_] = data;
+        end_ = end_ + 1;
+        count = count + 1;
+        return INT16_MAX;
+    } else if (count == window-1) {
+        history[end_] = data;
+        for (int i = 0; i <= count; i++)
+            total = total + history[i];
+        count = count + 1;
+        end_ = 0;
+        front = 1;
+        prev_data = history[0];
+        return INT16_MAX;
+    } else {
+        history[end_] = data;
+        total = total - prev_data + data;
+        end_ = end_ + 1;
+        front = front + 1;
+
+        if (end_ >= window)
+            end_ = 0;
+
+        if (front >= window)
+            front = 0;
+
+        if (front == 0)
+            prev_data = history[window-1];
+        else {
+            prev_data = history[front - 1];
+        }
+
+        return total/window;
+    }
+}
+
+/* Returns true if there is a peak (flip turn) */
+bool peak_detect(int16_t data)
+{
+    static uint8_t i = 0;
+    static int16_t avg[3];
+
+    int16_t m = mov_avg(data);
+
+    if (m == INT16_MAX)
+        return false;
+
+    if (i < 3) {
+        avg[i++] = m;
+        return false;
+    }
+
+    avg[0] = avg[1];
+    avg[1] = avg[2];
+    avg[2] = m;
+
+    if ( (avg[0] > avg[1]) && (avg[2] > avg[1]) && (avg[2] < -2000) && (avg[1] < -2000) && (avg[0] < -2000) ) {
+        PC_PRINTLNF("Peak detected @ %d", split_timer.read_ms());
+        return true;
+    }
+
+    return false;
+}
+
+enum length {ZERO, ONE, TWO};
+
+/* Returns split time, or UINT16_MAX if not on a split */
+uint16_t process_data(int16_t data)
+{
+    static enum length Length = ZERO;
+    static int l_zero, l_two;
+
+    if (peak_detect(data)) {
+        if (Length == ZERO) {
+            l_zero = split_timer.read_ms();
+            Length = ONE;
+            return UINT16_MAX;
+        } else if (Length == ONE) {
+//            l_one = SystemTime::read_ms();
+            Length = TWO;
+            return UINT16_MAX;
+        } else {
+            l_two = split_timer.read_ms();
+            Length = ZERO;
+            split_timer.reset();
+            return ( abs((l_two - l_zero)));
+        }
+    }
+    
+    return UINT16_MAX;
+}
+
+bool process_close() {
+    delete(history);
+    split_timer.reset();
+    split_timer.stop();
+    return true;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/process_data.h	Tue May 20 00:43:55 2014 +0000
@@ -0,0 +1,15 @@
+#ifndef _PROCESS_DATA_H
+#define _PROCESS_DATA_H
+
+#include "mbed.h"
+
+#define INT16_MAX 0x7FFF
+#define UINT16_MAX 0xFFFF
+
+#define WINDOW_SIZE 30
+
+bool process_init();
+uint16_t process_data(int16_t data);
+bool process_close();
+
+#endif // _PROCESS_DATA_H
\ No newline at end of file

--- a/receive_data.cpp	Sat May 17 01:20:31 2014 +0000
+++ b/receive_data.cpp	Tue May 20 00:43:55 2014 +0000
@@ -15,26 +15,27 @@
 uint8_t fifoBuffer[64]; // FIFO storage buffer
 
 // orientation/motion vars
-Quaternion q;           // [w, x, y, z]         quaternion container
+//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
+//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
 
 volatile bool mpuInterrupt = false;
 void dataReadyISR() {
     mpuInterrupt = true;
 }
 
-/* Returns a pointer to an array containing the most recent data, otherwise NULL.  Requires the mpu to be initialized. */
-void receive_data()
+/* Returns a pointer to an array containing the most recent data.  Will return NULL if unable to 
+   get new data (expect this to happen).  Requires the mpu to be initialized. */
+VectorInt16 *receive_data()
 {
     static uint32_t n_overflows = 0;
-//    while (true) {
-    //if (!dmpReady) break;   // do nothing if dmp not ready
-    if (!dmpReady) return;
+
+    // do nothing if dmp not ready
+    if (!dmpReady) return NULL;
 
     while (!mpuInterrupt && fifoCount < packetSize);
 
@@ -45,15 +46,14 @@
     // get current FIFO count
     fifoCount = mpu.getFIFOCount();
     
-//    PC_PRINTF("%d, ", fifoCount); PC_PRINTF("%x, ", mpuIntStatus);
     // check for overflow (this should never happen unless our code is too inefficient)
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         PC_PRINTLN("**** FIFO OVERFLOW ****");
         n_overflows++;
         // reset so we can continue cleanly
         mpu.resetFIFO();
-        // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
+        // otherwise, check for DMP data ready interrupt or another packet to process (this should happen frequently)
+    } else if (mpuIntStatus & 0x02 || fifoCount > packetSize) {
         // Wait for a full packet - should be very short wait
         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
 
@@ -62,23 +62,22 @@
         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, ", aa.x);
-        PC_PRINTF("%d, ", aa.y);
-        PC_PRINTLNF("%d", aa.z);
-//            OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
-
-//            fprintf(logFile, "%u,%d,%d,%d,%f,%f,%f,%f,%u\n", captureTime.read_ms(), aa.x, aa.y, aa.z, q.x, q.y, q.z, q.w, n_overflows);
-//        }
+        mpu.dmpGetAccel(&aa, fifoBuffer); 
+        return &aa;
     }
+    
+    return NULL;
 }
 
-void receive_init() {
+bool receive_close() {
+    // Disable DMP and clear fifo
+    mpu.setDMPEnabled(false);
+    mpu.resetFIFO();
+    
+    return true;
+}
+
+bool receive_init() {
     PC_PRINTLN("Initializing MPU");
     mpu.initialize();
     devStatus = mpu.dmpInitialize();
@@ -90,8 +89,10 @@
         PC_PRINTLN("DMP Initialized successfully!");
         dmpReady = true;
         dataReady.rise(dataReadyISR);
+        return true;
     } else { // ERROR
         PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
         DIE(DMP_ERROR_RATE);
+        return false;
     }
 }
\ No newline at end of file

--- a/receive_data.h	Sat May 17 01:20:31 2014 +0000
+++ b/receive_data.h	Tue May 20 00:43:55 2014 +0000
@@ -4,6 +4,7 @@
 #include "helper_3dmath.h"
 
 void receive_init();
-void receive_data();
+VectorInt16 *receive_data();
+bool receive_close();
 
 #endif // _RECEIVE_H
\ No newline at end of file