CSE477 / swimate_v2

Swimate V2 without RTOS code

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

main.cpp@4:b962f5a783a1, 2014-05-12 (annotated)

Committer:
ellingjp
Date:
Mon May 12 18:52:45 2014 +0000
Revision:
4:b962f5a783a1
Parent:
0:cd1fe4f0ed39
Child:
5:7c2e7d657716
Child:
7:33a74adff0ff
Fixed bug in switch debouncing related to the timer overflowing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ellingjp 0:cd1fe4f0ed39 1 #include "main.h"
ellingjp 0:cd1fe4f0ed39 2 #include "mbed.h"
ellingjp 0:cd1fe4f0ed39 3 #include "USBSerial.h"
ellingjp 0:cd1fe4f0ed39 4 #include "Adafruit_SSD1306.h"
ellingjp 0:cd1fe4f0ed39 5 #include "MPU6050_6Axis_MotionApps20.h"
ellingjp 0:cd1fe4f0ed39 6 #include "SDFileSystem.h"
ellingjp 0:cd1fe4f0ed39 7
ellingjp 0:cd1fe4f0ed39 8 //Virtual serial port over USB
ellingjp 0:cd1fe4f0ed39 9 #ifdef PC_DEBUG
ellingjp 0:cd1fe4f0ed39 10 USBSerial pc;
ellingjp 0:cd1fe4f0ed39 11 #endif
ellingjp 0:cd1fe4f0ed39 12
ellingjp 0:cd1fe4f0ed39 13 // Display
ellingjp 0:cd1fe4f0ed39 14 #ifdef OLED_DEBUG
ellingjp 0:cd1fe4f0ed39 15 SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk
ellingjp 4:b962f5a783a1 16 Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // DC, RST, CS
ellingjp 0:cd1fe4f0ed39 17 #endif
ellingjp 0:cd1fe4f0ed39 18
ellingjp 0:cd1fe4f0ed39 19 // MPU
ellingjp 0:cd1fe4f0ed39 20 MPU6050 mpu;
ellingjp 0:cd1fe4f0ed39 21 InterruptIn dataReady(P0_15);
ellingjp 0:cd1fe4f0ed39 22
ellingjp 0:cd1fe4f0ed39 23 // SD Card
ellingjp 0:cd1fe4f0ed39 24 SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1
ellingjp 0:cd1fe4f0ed39 25
ellingjp 0:cd1fe4f0ed39 26 // LED for debug
ellingjp 0:cd1fe4f0ed39 27 DigitalOut led(LED1);
ellingjp 0:cd1fe4f0ed39 28
ellingjp 0:cd1fe4f0ed39 29 // Logging vars
ellingjp 0:cd1fe4f0ed39 30 FILE *logFile;
ellingjp 0:cd1fe4f0ed39 31
ellingjp 0:cd1fe4f0ed39 32 // Timer
ellingjp 0:cd1fe4f0ed39 33 Timer totalTime;
ellingjp 0:cd1fe4f0ed39 34 Timer captureTime;
ellingjp 0:cd1fe4f0ed39 35
ellingjp 0:cd1fe4f0ed39 36 // Switch
ellingjp 0:cd1fe4f0ed39 37 InterruptIn captureSwitch(P0_16);
ellingjp 0:cd1fe4f0ed39 38
ellingjp 0:cd1fe4f0ed39 39
ellingjp 0:cd1fe4f0ed39 40 // State
ellingjp 0:cd1fe4f0ed39 41 enum state {IDLE, CAPTURE};
ellingjp 0:cd1fe4f0ed39 42 enum state State;
ellingjp 0:cd1fe4f0ed39 43
ellingjp 0:cd1fe4f0ed39 44 // MPU control/status vars
ellingjp 0:cd1fe4f0ed39 45 bool dmpReady = false; // set true if DMP init was successful
ellingjp 0:cd1fe4f0ed39 46 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
ellingjp 0:cd1fe4f0ed39 47 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
ellingjp 0:cd1fe4f0ed39 48 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
ellingjp 0:cd1fe4f0ed39 49 uint16_t fifoCount; // count of all bytes currently in FIFO
ellingjp 0:cd1fe4f0ed39 50 uint8_t fifoBuffer[64]; // FIFO storage buffer
ellingjp 0:cd1fe4f0ed39 51
ellingjp 0:cd1fe4f0ed39 52 // orientation/motion vars
ellingjp 0:cd1fe4f0ed39 53 Quaternion q; // [w, x, y, z] quaternion container
ellingjp 0:cd1fe4f0ed39 54 VectorInt16 aa; // [x, y, z] accel sensor measurements
ellingjp 0:cd1fe4f0ed39 55 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
ellingjp 0:cd1fe4f0ed39 56 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
ellingjp 0:cd1fe4f0ed39 57 VectorFloat gravity; // [x, y, z] gravity vector
ellingjp 0:cd1fe4f0ed39 58 float euler[3]; // [psi, theta, phi] Euler angle container
ellingjp 0:cd1fe4f0ed39 59 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
ellingjp 0:cd1fe4f0ed39 60
ellingjp 0:cd1fe4f0ed39 61 // Forward declarations
ellingjp 0:cd1fe4f0ed39 62 void die(int flash_rate_s);
ellingjp 0:cd1fe4f0ed39 63 bool log_open();
ellingjp 4:b962f5a783a1 64 void log_data(VectorInt16 data, Quaternion q);
ellingjp 0:cd1fe4f0ed39 65 void log_close();
ellingjp 0:cd1fe4f0ed39 66 void mpu_init();
ellingjp 0:cd1fe4f0ed39 67 void get_data();
ellingjp 0:cd1fe4f0ed39 68
ellingjp 0:cd1fe4f0ed39 69 volatile bool mpuInterrupt = false;
ellingjp 0:cd1fe4f0ed39 70 void dataReadyISR() {
ellingjp 0:cd1fe4f0ed39 71 mpuInterrupt = true;
ellingjp 0:cd1fe4f0ed39 72 }
ellingjp 0:cd1fe4f0ed39 73
ellingjp 4:b962f5a783a1 74 //void captureSwitchISR() {
ellingjp 4:b962f5a783a1 75 // // used for debouncing
ellingjp 4:b962f5a783a1 76 // static int prev_time = 0;
ellingjp 4:b962f5a783a1 77 //
ellingjp 4:b962f5a783a1 78 // if (totalTime.read_ms() - prev_time < 200)
ellingjp 4:b962f5a783a1 79 // return;
ellingjp 4:b962f5a783a1 80 //
ellingjp 4:b962f5a783a1 81 // State = (State == IDLE) ? CAPTURE : IDLE;
ellingjp 4:b962f5a783a1 82 // prev_time = totalTime.read_ms();
ellingjp 4:b962f5a783a1 83 //}
ellingjp 4:b962f5a783a1 84
ellingjp 0:cd1fe4f0ed39 85 void captureSwitchISR() {
ellingjp 0:cd1fe4f0ed39 86 // used for debouncing
ellingjp 0:cd1fe4f0ed39 87 static int prev_time = 0;
ellingjp 4:b962f5a783a1 88 int curr_time = totalTime.read_ms();
ellingjp 4:b962f5a783a1 89
ellingjp 4:b962f5a783a1 90 // Only change state after an amount of time
ellingjp 4:b962f5a783a1 91 // Note: abs value is necessary in case of
ellingjp 4:b962f5a783a1 92 // overflows
ellingjp 4:b962f5a783a1 93 if (abs(curr_time - prev_time) > 200)
ellingjp 4:b962f5a783a1 94 State = (State == IDLE) ? CAPTURE : IDLE;
ellingjp 0:cd1fe4f0ed39 95
ellingjp 4:b962f5a783a1 96 prev_time = curr_time;
ellingjp 0:cd1fe4f0ed39 97 }
ellingjp 0:cd1fe4f0ed39 98
ellingjp 0:cd1fe4f0ed39 99 int main(void)
ellingjp 0:cd1fe4f0ed39 100 {
ellingjp 0:cd1fe4f0ed39 101 totalTime.start();
ellingjp 0:cd1fe4f0ed39 102
ellingjp 0:cd1fe4f0ed39 103 State = IDLE;
ellingjp 0:cd1fe4f0ed39 104 captureSwitch.mode(PullUp);
ellingjp 0:cd1fe4f0ed39 105 captureSwitch.rise(captureSwitchISR);
ellingjp 0:cd1fe4f0ed39 106
ellingjp 0:cd1fe4f0ed39 107 OLED_PRINTP("Waiting to capture. ",0,0);
ellingjp 0:cd1fe4f0ed39 108 while (true) {
ellingjp 0:cd1fe4f0ed39 109 if (State == CAPTURE) {
ellingjp 0:cd1fe4f0ed39 110 PC_PRINTLN("Start capture button pressed!");
ellingjp 0:cd1fe4f0ed39 111 OLED_CLEAR();
ellingjp 0:cd1fe4f0ed39 112 OLED_PRINTPR("Starting capture. ",0,0);
ellingjp 0:cd1fe4f0ed39 113
ellingjp 0:cd1fe4f0ed39 114 // Start receiving data
ellingjp 0:cd1fe4f0ed39 115 PC_PRINTLN("Opening log file...");
ellingjp 0:cd1fe4f0ed39 116 if (!log_open()) {
ellingjp 0:cd1fe4f0ed39 117 OLED_CLEAR();
ellingjp 0:cd1fe4f0ed39 118 OLED_PRINTP("ERROR: SD (retry)", 0, 50);
ellingjp 0:cd1fe4f0ed39 119 State = IDLE;
ellingjp 0:cd1fe4f0ed39 120 continue;
ellingjp 0:cd1fe4f0ed39 121 }
ellingjp 0:cd1fe4f0ed39 122
ellingjp 0:cd1fe4f0ed39 123 PC_PRINTLN("Initializing MPU...");
ellingjp 0:cd1fe4f0ed39 124 mpu_init();
ellingjp 0:cd1fe4f0ed39 125
ellingjp 0:cd1fe4f0ed39 126 PC_PRINTLN("Starting capture...");
ellingjp 0:cd1fe4f0ed39 127 OLED_PRINTPR("Capturing data... ",0,0);
ellingjp 0:cd1fe4f0ed39 128 captureTime.start();
ellingjp 0:cd1fe4f0ed39 129 while (State == CAPTURE)
ellingjp 0:cd1fe4f0ed39 130 get_data();
ellingjp 0:cd1fe4f0ed39 131 OLED_PRINTPR("Finished capture.",0,0);
ellingjp 0:cd1fe4f0ed39 132
ellingjp 0:cd1fe4f0ed39 133 log_close();
ellingjp 0:cd1fe4f0ed39 134 captureTime.stop();
ellingjp 4:b962f5a783a1 135 captureTime.reset();
ellingjp 0:cd1fe4f0ed39 136 }
ellingjp 0:cd1fe4f0ed39 137
ellingjp 0:cd1fe4f0ed39 138 PC_PRINTLN("Idling...");
ellingjp 0:cd1fe4f0ed39 139 }
ellingjp 0:cd1fe4f0ed39 140 }
ellingjp 0:cd1fe4f0ed39 141
ellingjp 0:cd1fe4f0ed39 142 /* Halts program and flashes LED at specified rate */
ellingjp 0:cd1fe4f0ed39 143 void die(int flash_rate_s) {
ellingjp 0:cd1fe4f0ed39 144 while (1) {
ellingjp 0:cd1fe4f0ed39 145 led = 1;
ellingjp 0:cd1fe4f0ed39 146 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 147 led = 0;
ellingjp 0:cd1fe4f0ed39 148 wait(flash_rate_s/2);
ellingjp 0:cd1fe4f0ed39 149 }
ellingjp 0:cd1fe4f0ed39 150 }
ellingjp 0:cd1fe4f0ed39 151
ellingjp 0:cd1fe4f0ed39 152 /* Returns false on failure, true otherwise */
ellingjp 0:cd1fe4f0ed39 153 bool log_open() {
ellingjp 0:cd1fe4f0ed39 154 logFile = fopen(LOG_FILE, "a");
ellingjp 0:cd1fe4f0ed39 155 if (logFile == NULL) {
ellingjp 0:cd1fe4f0ed39 156 PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE);
ellingjp 0:cd1fe4f0ed39 157 return false;
ellingjp 0:cd1fe4f0ed39 158 }
ellingjp 0:cd1fe4f0ed39 159 fprintf(logFile, "---- BEGIN NEW DATASET ----\n");
ellingjp 0:cd1fe4f0ed39 160 return true;
ellingjp 0:cd1fe4f0ed39 161 }
ellingjp 0:cd1fe4f0ed39 162
ellingjp 0:cd1fe4f0ed39 163 void log_close() {
ellingjp 0:cd1fe4f0ed39 164 if (logFile != NULL)
ellingjp 0:cd1fe4f0ed39 165 fclose(logFile);
ellingjp 0:cd1fe4f0ed39 166 }
ellingjp 0:cd1fe4f0ed39 167
ellingjp 0:cd1fe4f0ed39 168 void mpu_init() {
ellingjp 0:cd1fe4f0ed39 169 PC_PRINTLN("Initializing MPU");
ellingjp 0:cd1fe4f0ed39 170 mpu.initialize();
ellingjp 0:cd1fe4f0ed39 171 devStatus = mpu.dmpInitialize();
ellingjp 0:cd1fe4f0ed39 172
ellingjp 0:cd1fe4f0ed39 173 if (devStatus == 0) {
ellingjp 0:cd1fe4f0ed39 174 mpu.setDMPEnabled(true);
ellingjp 0:cd1fe4f0ed39 175 packetSize = mpu.dmpGetFIFOPacketSize();
ellingjp 0:cd1fe4f0ed39 176
ellingjp 0:cd1fe4f0ed39 177 PC_PRINTLN("DMP Initialized successfully!");
ellingjp 0:cd1fe4f0ed39 178 dmpReady = true;
ellingjp 0:cd1fe4f0ed39 179 dataReady.rise(dataReadyISR);
ellingjp 0:cd1fe4f0ed39 180 } else { // ERROR
ellingjp 0:cd1fe4f0ed39 181 PC_PRINTLNF("Error initializing MPU (code %d)", devStatus);
ellingjp 0:cd1fe4f0ed39 182 die(DMP_ERROR_RATE);
ellingjp 0:cd1fe4f0ed39 183 }
ellingjp 0:cd1fe4f0ed39 184 }
ellingjp 0:cd1fe4f0ed39 185
ellingjp 0:cd1fe4f0ed39 186 /* Requires the log to be open and mpu to be initialized*/
ellingjp 0:cd1fe4f0ed39 187 void get_data() {
ellingjp 4:b962f5a783a1 188 static uint32_t n_overflows = 0;
ellingjp 0:cd1fe4f0ed39 189 //while (true) {
ellingjp 0:cd1fe4f0ed39 190 //if (!dmpReady) break; // do nothing if dmp not ready
ellingjp 0:cd1fe4f0ed39 191 if (!dmpReady) return;
ellingjp 0:cd1fe4f0ed39 192
ellingjp 0:cd1fe4f0ed39 193 while (!mpuInterrupt && fifoCount < packetSize);
ellingjp 0:cd1fe4f0ed39 194
ellingjp 0:cd1fe4f0ed39 195 // Reset interrupt
ellingjp 0:cd1fe4f0ed39 196 mpuInterrupt = false;
ellingjp 0:cd1fe4f0ed39 197 mpuIntStatus = mpu.getIntStatus();
ellingjp 0:cd1fe4f0ed39 198
ellingjp 0:cd1fe4f0ed39 199 // get current FIFO count
ellingjp 0:cd1fe4f0ed39 200 fifoCount = mpu.getFIFOCount();
ellingjp 0:cd1fe4f0ed39 201
ellingjp 0:cd1fe4f0ed39 202 // check for overflow (this should never happen unless our code is too inefficient)
ellingjp 0:cd1fe4f0ed39 203 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
ellingjp 0:cd1fe4f0ed39 204 PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms());
ellingjp 4:b962f5a783a1 205 n_overflows++;
ellingjp 0:cd1fe4f0ed39 206 // reset so we can continue cleanly
ellingjp 0:cd1fe4f0ed39 207 mpu.resetFIFO();
ellingjp 0:cd1fe4f0ed39 208 // otherwise, check for DMP data ready interrupt (this should happen frequently)
ellingjp 0:cd1fe4f0ed39 209 } else if (mpuIntStatus & 0x02) {
ellingjp 0:cd1fe4f0ed39 210 // Wait for a full packet - should be very short wait
ellingjp 0:cd1fe4f0ed39 211 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
ellingjp 0:cd1fe4f0ed39 212
ellingjp 0:cd1fe4f0ed39 213 // read a packet from FIFO
ellingjp 0:cd1fe4f0ed39 214 mpu.getFIFOBytes(fifoBuffer, packetSize);
ellingjp 0:cd1fe4f0ed39 215 fifoCount -= packetSize;
ellingjp 0:cd1fe4f0ed39 216
ellingjp 0:cd1fe4f0ed39 217 // Get acceleration data
ellingjp 0:cd1fe4f0ed39 218 mpu.dmpGetAccel(&aa, fifoBuffer);
ellingjp 0:cd1fe4f0ed39 219 mpu.dmpGetQuaternion(&q, fifoBuffer);
ellingjp 4:b962f5a783a1 220 // mpu.dmpGetGravity(&gravity, &q);
ellingjp 4:b962f5a783a1 221 // mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
ellingjp 4:b962f5a783a1 222 // mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
ellingjp 0:cd1fe4f0ed39 223
ellingjp 0:cd1fe4f0ed39 224 PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z);
ellingjp 0:cd1fe4f0ed39 225
ellingjp 0:cd1fe4f0ed39 226 // OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z);
ellingjp 0:cd1fe4f0ed39 227
ellingjp 4:b962f5a783a1 228 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);
ellingjp 0:cd1fe4f0ed39 229 //}
ellingjp 0:cd1fe4f0ed39 230 }
ellingjp 0:cd1fe4f0ed39 231 }
ellingjp 0:cd1fe4f0ed39 232
ellingjp 4:b962f5a783a1 233 void log_data(VectorInt16 data, Quaternion q) {
ellingjp 4:b962f5a783a1 234 fprintf(logFile, "%d,%d,%d,%d,%f,%f,%f,%f\n", captureTime.read_ms(), data.x, data.y, data.z, q.x, q.y, q.z, q.w);
ellingjp 0:cd1fe4f0ed39 235 }