Swimate V2 without RTOS code
Dependencies: Adafruit_GFX_128x64 DS3231 PinDetect SDFileSystem USBDevice mbed RealtimeMath MODSERIAL
main.cpp
- Committer:
- ellingjp
- Date:
- 2014-05-14
- Revision:
- 5:7c2e7d657716
- Parent:
- 4:b962f5a783a1
- Child:
- 6:3b9b4e2c29bf
File content as of revision 5:7c2e7d657716:
#include "mbed.h" #include "rtos.h" #include "USBSerial.h" #include "Adafruit_SSD1306.h" #include "main.h" #include "capture.h" #include "debug.h" #define SIG_IDLE 0x1 // Display #ifdef OLED_DEBUG SPI spi0(P0_9, NC, P0_10); // mosi, miso, sclk Adafruit_SSD1306 oled(spi0, P0_11, P0_12, P0_13); // DC, RST, CS #endif // Switch InterruptIn modeSwitch(P0_16); // Mode enum mode {IDLE, CAPTURE}; enum mode Mode; // Timer Timer totalTime; // Main Thread osThreadId mainThreadId; void modeSwInterrupt() { // used for debouncing static int prev_time = 0; int curr_time = totalTime.read_ms(); // Only change state after an amount of time // Note: abs value is necessary in case of overflows if (abs(curr_time - prev_time) > 200) { if (Mode != IDLE) { osSignalSet(mainThreadId, SIG_IDLE); Mode = IDLE; } else Mode = CAPTURE; } prev_time = curr_time; } int main() { Mode = IDLE; modeSwitch.mode(PullUp); modeSwitch.rise(modeSwInterrupt); mainThreadId = osThreadGetId(); totalTime.start(); while (true) { if (Mode == CAPTURE) { PC_PRINTLN("Initializing capture mode..."); captureThrInit(); PC_PRINTLN("Starting capture..."); Thread captureThr(captureThr); PC_PRINTLN("Capturing..."); Thread::signal_wait(SIG_IDLE); PC_PRINTLN("Closing capture thread..."); captureThrClose(); captureThr.terminate(); PC_PRINTLN("Capture complete."); } PC_PRINTLN("Idling..."); } } ////Virtual serial port over USB //#ifdef PC_DEBUG //USBSerial pc; //#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_data(VectorInt16 data, Quaternion q); //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; // int curr_time = totalTime.read_ms(); // // // Only change state after an amount of time // // Note: abs value is necessary in case of // // overflows // if (abs(curr_time - prev_time) > 200) // State = (State == IDLE) ? CAPTURE : IDLE; // // prev_time = curr_time; //} // //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(); // // // Start receiving data // Thread get_data_thr(get_data); // // // Wait for capture button // Thread::wait(SIG_CAPTURE); // // // Stop receiving data // get_data_thr.terminate(); // // OLED_PRINTPR("Finished capture.",0,0); // // log_close(); // captureTime.stop(); // captureTime.reset(); // } // // 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() { // static uint32_t n_overflows = 0; // //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()); // n_overflows++; // // 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); // // 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); // //} // } //} // //void log_data(VectorInt16 data, Quaternion q) { // 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); //}