Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
main.cpp@0:cd1fe4f0ed39, 2014-05-08 (annotated)
- Committer:
- ellingjp
- Date:
- Thu May 08 20:01:10 2014 +0000
- Revision:
- 0:cd1fe4f0ed39
- Child:
- 1:93723df61425
- Child:
- 2:a28cc676a454
- Child:
- 4:b962f5a783a1
Initial commit. Ready for data gathering.
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:cd1fe4f0ed39 | 16 | Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0 |
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 | 0:cd1fe4f0ed39 | 64 | void log_acceleration(VectorInt16 data); |
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 | 0:cd1fe4f0ed39 | 74 | void captureSwitchISR() { |
ellingjp | 0:cd1fe4f0ed39 | 75 | // used for debouncing |
ellingjp | 0:cd1fe4f0ed39 | 76 | static int prev_time = 0; |
ellingjp | 0:cd1fe4f0ed39 | 77 | |
ellingjp | 0:cd1fe4f0ed39 | 78 | if (totalTime.read_ms() - prev_time < 200) |
ellingjp | 0:cd1fe4f0ed39 | 79 | return; |
ellingjp | 0:cd1fe4f0ed39 | 80 | |
ellingjp | 0:cd1fe4f0ed39 | 81 | State = (State == IDLE) ? CAPTURE : IDLE; |
ellingjp | 0:cd1fe4f0ed39 | 82 | prev_time = totalTime.read_ms(); |
ellingjp | 0:cd1fe4f0ed39 | 83 | } |
ellingjp | 0:cd1fe4f0ed39 | 84 | |
ellingjp | 0:cd1fe4f0ed39 | 85 | int main(void) |
ellingjp | 0:cd1fe4f0ed39 | 86 | { |
ellingjp | 0:cd1fe4f0ed39 | 87 | totalTime.start(); |
ellingjp | 0:cd1fe4f0ed39 | 88 | |
ellingjp | 0:cd1fe4f0ed39 | 89 | State = IDLE; |
ellingjp | 0:cd1fe4f0ed39 | 90 | captureSwitch.mode(PullUp); |
ellingjp | 0:cd1fe4f0ed39 | 91 | captureSwitch.rise(captureSwitchISR); |
ellingjp | 0:cd1fe4f0ed39 | 92 | |
ellingjp | 0:cd1fe4f0ed39 | 93 | OLED_PRINTP("Waiting to capture. ",0,0); |
ellingjp | 0:cd1fe4f0ed39 | 94 | while (true) { |
ellingjp | 0:cd1fe4f0ed39 | 95 | if (State == CAPTURE) { |
ellingjp | 0:cd1fe4f0ed39 | 96 | PC_PRINTLN("Start capture button pressed!"); |
ellingjp | 0:cd1fe4f0ed39 | 97 | OLED_CLEAR(); |
ellingjp | 0:cd1fe4f0ed39 | 98 | OLED_PRINTPR("Starting capture. ",0,0); |
ellingjp | 0:cd1fe4f0ed39 | 99 | |
ellingjp | 0:cd1fe4f0ed39 | 100 | // Start receiving data |
ellingjp | 0:cd1fe4f0ed39 | 101 | PC_PRINTLN("Opening log file..."); |
ellingjp | 0:cd1fe4f0ed39 | 102 | if (!log_open()) { |
ellingjp | 0:cd1fe4f0ed39 | 103 | OLED_CLEAR(); |
ellingjp | 0:cd1fe4f0ed39 | 104 | OLED_PRINTP("ERROR: SD (retry)", 0, 50); |
ellingjp | 0:cd1fe4f0ed39 | 105 | State = IDLE; |
ellingjp | 0:cd1fe4f0ed39 | 106 | continue; |
ellingjp | 0:cd1fe4f0ed39 | 107 | } |
ellingjp | 0:cd1fe4f0ed39 | 108 | |
ellingjp | 0:cd1fe4f0ed39 | 109 | PC_PRINTLN("Initializing MPU..."); |
ellingjp | 0:cd1fe4f0ed39 | 110 | mpu_init(); |
ellingjp | 0:cd1fe4f0ed39 | 111 | |
ellingjp | 0:cd1fe4f0ed39 | 112 | PC_PRINTLN("Starting capture..."); |
ellingjp | 0:cd1fe4f0ed39 | 113 | OLED_PRINTPR("Capturing data... ",0,0); |
ellingjp | 0:cd1fe4f0ed39 | 114 | captureTime.start(); |
ellingjp | 0:cd1fe4f0ed39 | 115 | while (State == CAPTURE) |
ellingjp | 0:cd1fe4f0ed39 | 116 | get_data(); |
ellingjp | 0:cd1fe4f0ed39 | 117 | OLED_PRINTPR("Finished capture.",0,0); |
ellingjp | 0:cd1fe4f0ed39 | 118 | |
ellingjp | 0:cd1fe4f0ed39 | 119 | log_close(); |
ellingjp | 0:cd1fe4f0ed39 | 120 | captureTime.stop(); |
ellingjp | 0:cd1fe4f0ed39 | 121 | } |
ellingjp | 0:cd1fe4f0ed39 | 122 | |
ellingjp | 0:cd1fe4f0ed39 | 123 | PC_PRINTLN("Idling..."); |
ellingjp | 0:cd1fe4f0ed39 | 124 | } |
ellingjp | 0:cd1fe4f0ed39 | 125 | } |
ellingjp | 0:cd1fe4f0ed39 | 126 | |
ellingjp | 0:cd1fe4f0ed39 | 127 | /* Halts program and flashes LED at specified rate */ |
ellingjp | 0:cd1fe4f0ed39 | 128 | void die(int flash_rate_s) { |
ellingjp | 0:cd1fe4f0ed39 | 129 | while (1) { |
ellingjp | 0:cd1fe4f0ed39 | 130 | led = 1; |
ellingjp | 0:cd1fe4f0ed39 | 131 | wait(flash_rate_s/2); |
ellingjp | 0:cd1fe4f0ed39 | 132 | led = 0; |
ellingjp | 0:cd1fe4f0ed39 | 133 | wait(flash_rate_s/2); |
ellingjp | 0:cd1fe4f0ed39 | 134 | } |
ellingjp | 0:cd1fe4f0ed39 | 135 | } |
ellingjp | 0:cd1fe4f0ed39 | 136 | |
ellingjp | 0:cd1fe4f0ed39 | 137 | /* Returns false on failure, true otherwise */ |
ellingjp | 0:cd1fe4f0ed39 | 138 | bool log_open() { |
ellingjp | 0:cd1fe4f0ed39 | 139 | logFile = fopen(LOG_FILE, "a"); |
ellingjp | 0:cd1fe4f0ed39 | 140 | if (logFile == NULL) { |
ellingjp | 0:cd1fe4f0ed39 | 141 | PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE); |
ellingjp | 0:cd1fe4f0ed39 | 142 | return false; |
ellingjp | 0:cd1fe4f0ed39 | 143 | } |
ellingjp | 0:cd1fe4f0ed39 | 144 | fprintf(logFile, "---- BEGIN NEW DATASET ----\n"); |
ellingjp | 0:cd1fe4f0ed39 | 145 | return true; |
ellingjp | 0:cd1fe4f0ed39 | 146 | } |
ellingjp | 0:cd1fe4f0ed39 | 147 | |
ellingjp | 0:cd1fe4f0ed39 | 148 | void log_close() { |
ellingjp | 0:cd1fe4f0ed39 | 149 | if (logFile != NULL) |
ellingjp | 0:cd1fe4f0ed39 | 150 | fclose(logFile); |
ellingjp | 0:cd1fe4f0ed39 | 151 | } |
ellingjp | 0:cd1fe4f0ed39 | 152 | |
ellingjp | 0:cd1fe4f0ed39 | 153 | void mpu_init() { |
ellingjp | 0:cd1fe4f0ed39 | 154 | PC_PRINTLN("Initializing MPU"); |
ellingjp | 0:cd1fe4f0ed39 | 155 | mpu.initialize(); |
ellingjp | 0:cd1fe4f0ed39 | 156 | devStatus = mpu.dmpInitialize(); |
ellingjp | 0:cd1fe4f0ed39 | 157 | |
ellingjp | 0:cd1fe4f0ed39 | 158 | if (devStatus == 0) { |
ellingjp | 0:cd1fe4f0ed39 | 159 | mpu.setDMPEnabled(true); |
ellingjp | 0:cd1fe4f0ed39 | 160 | packetSize = mpu.dmpGetFIFOPacketSize(); |
ellingjp | 0:cd1fe4f0ed39 | 161 | |
ellingjp | 0:cd1fe4f0ed39 | 162 | PC_PRINTLN("DMP Initialized successfully!"); |
ellingjp | 0:cd1fe4f0ed39 | 163 | dmpReady = true; |
ellingjp | 0:cd1fe4f0ed39 | 164 | dataReady.rise(dataReadyISR); |
ellingjp | 0:cd1fe4f0ed39 | 165 | } else { // ERROR |
ellingjp | 0:cd1fe4f0ed39 | 166 | PC_PRINTLNF("Error initializing MPU (code %d)", devStatus); |
ellingjp | 0:cd1fe4f0ed39 | 167 | die(DMP_ERROR_RATE); |
ellingjp | 0:cd1fe4f0ed39 | 168 | } |
ellingjp | 0:cd1fe4f0ed39 | 169 | } |
ellingjp | 0:cd1fe4f0ed39 | 170 | |
ellingjp | 0:cd1fe4f0ed39 | 171 | /* Requires the log to be open and mpu to be initialized*/ |
ellingjp | 0:cd1fe4f0ed39 | 172 | void get_data() { |
ellingjp | 0:cd1fe4f0ed39 | 173 | //while (true) { |
ellingjp | 0:cd1fe4f0ed39 | 174 | //if (!dmpReady) break; // do nothing if dmp not ready |
ellingjp | 0:cd1fe4f0ed39 | 175 | if (!dmpReady) return; |
ellingjp | 0:cd1fe4f0ed39 | 176 | |
ellingjp | 0:cd1fe4f0ed39 | 177 | while (!mpuInterrupt && fifoCount < packetSize); |
ellingjp | 0:cd1fe4f0ed39 | 178 | |
ellingjp | 0:cd1fe4f0ed39 | 179 | // Reset interrupt |
ellingjp | 0:cd1fe4f0ed39 | 180 | mpuInterrupt = false; |
ellingjp | 0:cd1fe4f0ed39 | 181 | mpuIntStatus = mpu.getIntStatus(); |
ellingjp | 0:cd1fe4f0ed39 | 182 | |
ellingjp | 0:cd1fe4f0ed39 | 183 | // get current FIFO count |
ellingjp | 0:cd1fe4f0ed39 | 184 | fifoCount = mpu.getFIFOCount(); |
ellingjp | 0:cd1fe4f0ed39 | 185 | |
ellingjp | 0:cd1fe4f0ed39 | 186 | // check for overflow (this should never happen unless our code is too inefficient) |
ellingjp | 0:cd1fe4f0ed39 | 187 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { |
ellingjp | 0:cd1fe4f0ed39 | 188 | PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms()); |
ellingjp | 0:cd1fe4f0ed39 | 189 | // reset so we can continue cleanly |
ellingjp | 0:cd1fe4f0ed39 | 190 | mpu.resetFIFO(); |
ellingjp | 0:cd1fe4f0ed39 | 191 | // otherwise, check for DMP data ready interrupt (this should happen frequently) |
ellingjp | 0:cd1fe4f0ed39 | 192 | } else if (mpuIntStatus & 0x02) { |
ellingjp | 0:cd1fe4f0ed39 | 193 | // Wait for a full packet - should be very short wait |
ellingjp | 0:cd1fe4f0ed39 | 194 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); |
ellingjp | 0:cd1fe4f0ed39 | 195 | |
ellingjp | 0:cd1fe4f0ed39 | 196 | // read a packet from FIFO |
ellingjp | 0:cd1fe4f0ed39 | 197 | mpu.getFIFOBytes(fifoBuffer, packetSize); |
ellingjp | 0:cd1fe4f0ed39 | 198 | fifoCount -= packetSize; |
ellingjp | 0:cd1fe4f0ed39 | 199 | |
ellingjp | 0:cd1fe4f0ed39 | 200 | // Get acceleration data |
ellingjp | 0:cd1fe4f0ed39 | 201 | mpu.dmpGetAccel(&aa, fifoBuffer); |
ellingjp | 0:cd1fe4f0ed39 | 202 | mpu.dmpGetQuaternion(&q, fifoBuffer); |
ellingjp | 0:cd1fe4f0ed39 | 203 | mpu.dmpGetGravity(&gravity, &q); |
ellingjp | 0:cd1fe4f0ed39 | 204 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); |
ellingjp | 0:cd1fe4f0ed39 | 205 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); |
ellingjp | 0:cd1fe4f0ed39 | 206 | |
ellingjp | 0:cd1fe4f0ed39 | 207 | PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z); |
ellingjp | 0:cd1fe4f0ed39 | 208 | |
ellingjp | 0:cd1fe4f0ed39 | 209 | // OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z); |
ellingjp | 0:cd1fe4f0ed39 | 210 | |
ellingjp | 0:cd1fe4f0ed39 | 211 | log_acceleration(aaWorld); |
ellingjp | 0:cd1fe4f0ed39 | 212 | //} |
ellingjp | 0:cd1fe4f0ed39 | 213 | } |
ellingjp | 0:cd1fe4f0ed39 | 214 | } |
ellingjp | 0:cd1fe4f0ed39 | 215 | |
ellingjp | 0:cd1fe4f0ed39 | 216 | void log_acceleration(VectorInt16 data) { |
ellingjp | 0:cd1fe4f0ed39 | 217 | fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z); |
ellingjp | 0:cd1fe4f0ed39 | 218 | } |