CSE477 / swimate_v2

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?

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 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 }