Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
Diff: main.cpp
- Revision:
- 0:cd1fe4f0ed39
- Child:
- 1:93723df61425
- Child:
- 2:a28cc676a454
- Child:
- 4:b962f5a783a1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu May 08 20:01:10 2014 +0000 @@ -0,0 +1,218 @@ +#include "main.h" +#include "mbed.h" +#include "USBSerial.h" +#include "Adafruit_SSD1306.h" +#include "MPU6050_6Axis_MotionApps20.h" +#include "SDFileSystem.h" + +//Virtual serial port over USB +#ifdef PC_DEBUG +USBSerial pc; +#endif + +// Display +#ifdef OLED_DEBUG +SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk +Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // MOSI, MISO, SCLK SPI0 +#endif + +// MPU +MPU6050 mpu; +InterruptIn dataReady(P0_15); + +// SD Card +SDFileSystem sd(P0_21, P0_22, P1_15, P1_19, "sd"); // MOSI, MISO, SCLK, SSEL SPI1 + +// LED for debug +DigitalOut led(LED1); + +// Logging vars +FILE *logFile; + +// Timer +Timer totalTime; +Timer captureTime; + +// Switch +InterruptIn captureSwitch(P0_16); + + +// State +enum state {IDLE, CAPTURE}; +enum state State; + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +// Forward declarations +void die(int flash_rate_s); +bool log_open(); +void log_acceleration(VectorInt16 data); +void log_close(); +void mpu_init(); +void get_data(); + +volatile bool mpuInterrupt = false; +void dataReadyISR() { + mpuInterrupt = true; +} + +void captureSwitchISR() { + // used for debouncing + static int prev_time = 0; + + if (totalTime.read_ms() - prev_time < 200) + return; + + State = (State == IDLE) ? CAPTURE : IDLE; + prev_time = totalTime.read_ms(); +} + +int main(void) +{ + totalTime.start(); + + State = IDLE; + captureSwitch.mode(PullUp); + captureSwitch.rise(captureSwitchISR); + + OLED_PRINTP("Waiting to capture. ",0,0); + while (true) { + if (State == CAPTURE) { + PC_PRINTLN("Start capture button pressed!"); + OLED_CLEAR(); + OLED_PRINTPR("Starting capture. ",0,0); + + // Start receiving data + PC_PRINTLN("Opening log file..."); + if (!log_open()) { + OLED_CLEAR(); + OLED_PRINTP("ERROR: SD (retry)", 0, 50); + State = IDLE; + continue; + } + + PC_PRINTLN("Initializing MPU..."); + mpu_init(); + + PC_PRINTLN("Starting capture..."); + OLED_PRINTPR("Capturing data... ",0,0); + captureTime.start(); + while (State == CAPTURE) + get_data(); + OLED_PRINTPR("Finished capture.",0,0); + + log_close(); + captureTime.stop(); + } + + PC_PRINTLN("Idling..."); + } +} + +/* Halts program and flashes LED at specified rate */ +void die(int flash_rate_s) { + while (1) { + led = 1; + wait(flash_rate_s/2); + led = 0; + wait(flash_rate_s/2); + } +} + +/* Returns false on failure, true otherwise */ +bool log_open() { + logFile = fopen(LOG_FILE, "a"); + if (logFile == NULL) { + PC_PRINTLNF("SD card initialization error: Failed to open %s", LOG_FILE); + return false; + } + fprintf(logFile, "---- BEGIN NEW DATASET ----\n"); + return true; +} + +void log_close() { + if (logFile != NULL) + fclose(logFile); +} + +void mpu_init() { + PC_PRINTLN("Initializing MPU"); + mpu.initialize(); + devStatus = mpu.dmpInitialize(); + + if (devStatus == 0) { + mpu.setDMPEnabled(true); + packetSize = mpu.dmpGetFIFOPacketSize(); + + PC_PRINTLN("DMP Initialized successfully!"); + dmpReady = true; + dataReady.rise(dataReadyISR); + } else { // ERROR + PC_PRINTLNF("Error initializing MPU (code %d)", devStatus); + die(DMP_ERROR_RATE); + } +} + +/* Requires the log to be open and mpu to be initialized*/ +void get_data() { + //while (true) { + //if (!dmpReady) break; // do nothing if dmp not ready + if (!dmpReady) return; + + while (!mpuInterrupt && fifoCount < packetSize); + + // Reset interrupt + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + PC_PRINTLNF("**** FIFO OVERFLOW @ %d ms ****", captureTime.read_ms()); + // reset so we can continue cleanly + mpu.resetFIFO(); + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // Wait for a full packet - should be very short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + fifoCount -= packetSize; + + // Get acceleration data + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + + PC_PRINTF("%d, ", aaWorld.x); PC_PRINTF("%d, ", aaWorld.y); PC_PRINTLNF("%d", aaWorld.z); + +// OLED_SETCURS(0, 10); OLED_PRINTF("%d, ", aaWorld.x); OLED_PRINTF("%d, ", aaWorld.y); OLED_PRINTLNF("%d", aaWorld.z); + + log_acceleration(aaWorld); + //} + } +} + +void log_acceleration(VectorInt16 data) { + fprintf(logFile, "%d, %d,%d,%d\n", captureTime.read_ms(), data.x, data.y, data.z); +} \ No newline at end of file