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@6:3b9b4e2c29bf, 2014-05-16 (annotated)

Committer:
ellingjp
Date:
Fri May 16 23:32:49 2014 +0000
Revision:
6:3b9b4e2c29bf
Parent:
5:7c2e7d657716 
rtos breaks I2C.  Workaround is to disable interrupts when accessing i2c devices (undesirable).  SD library may also be broken.

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