CSE477 / swimate_v2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL

main.cpp@5:7c2e7d657716, 2014-05-14 (annotated)

Committer:
ellingjp
Date:
Wed May 14 03:14:57 2014 +0000
Revision:
5:7c2e7d657716
Parent:
4:b962f5a783a1
Child:
6:3b9b4e2c29bf
rtos integration

Who changed what in which revision?

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