Amador Valley High School Robotics Team / Mbed 2 deprecated SparkFun_razor_imu

A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.

Dependencies:   mbed

main.cpp@0:a260d84e07fc, 2011-10-16 (annotated)

Committer:
avbotz
Date:
Sun Oct 16 05:37:27 2011 +0000
Revision:
0:a260d84e07fc
Child:
1:fdfa313b9cc3
Sort of working IMU code. Relies on the IMU being in ascii mode.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
avbotz 0:a260d84e07fc 1 /*
avbotz 0:a260d84e07fc 2 * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current
avbotz 0:a260d84e07fc 3 * Updated to use interrupts - this will help when we intergrate this code into AVNavControl
avbotz 0:a260d84e07fc 4 * 9dof razor from sparkfun
avbotz 0:a260d84e07fc 5 */
avbotz 0:a260d84e07fc 6
avbotz 0:a260d84e07fc 7 #define GYRO_SCALE 14.375 // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf
avbotz 0:a260d84e07fc 8
avbotz 0:a260d84e07fc 9 #include "mbed.h"
avbotz 0:a260d84e07fc 10 #include <string>
avbotz 0:a260d84e07fc 11 #include <sstream>
avbotz 0:a260d84e07fc 12
avbotz 0:a260d84e07fc 13 Serial IMU(p9, p10); // tx, rx
avbotz 0:a260d84e07fc 14 Serial PC(USBTX, USBRX);
avbotz 0:a260d84e07fc 15
avbotz 0:a260d84e07fc 16 DigitalOut myled1(LED1);
avbotz 0:a260d84e07fc 17 DigitalOut myled2(LED2);
avbotz 0:a260d84e07fc 18 DigitalOut myled3(LED3);
avbotz 0:a260d84e07fc 19 DigitalOut myled4(LED4);
avbotz 0:a260d84e07fc 20
avbotz 0:a260d84e07fc 21 bool IMUreadable = false;
avbotz 0:a260d84e07fc 22 bool PCreadable = false;
avbotz 0:a260d84e07fc 23 bool parseNow = false;
avbotz 0:a260d84e07fc 24
avbotz 0:a260d84e07fc 25 bool debugMode = false;
avbotz 0:a260d84e07fc 26
avbotz 0:a260d84e07fc 27 string buffer;
avbotz 0:a260d84e07fc 28
avbotz 0:a260d84e07fc 29 void readIMU();
avbotz 0:a260d84e07fc 30 void readPC();
avbotz 0:a260d84e07fc 31
avbotz 0:a260d84e07fc 32 void parse(string);
avbotz 0:a260d84e07fc 33
avbotz 0:a260d84e07fc 34 int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
avbotz 0:a260d84e07fc 35
avbotz 0:a260d84e07fc 36 int main() {
avbotz 0:a260d84e07fc 37 IMU.format(8, Serial::None, 1);
avbotz 0:a260d84e07fc 38 PC.format(8, Serial::None, 1);
avbotz 0:a260d84e07fc 39 IMU.baud(57600);
avbotz 0:a260d84e07fc 40 PC.baud(115200);
avbotz 0:a260d84e07fc 41
avbotz 0:a260d84e07fc 42 PC.printf("----------------------------\n\r");
avbotz 0:a260d84e07fc 43 PC.printf("debug: device reset\n\r");
avbotz 0:a260d84e07fc 44 PC.printf("debug: gotta have my bowl; gotta have serial. serial is up.\n\r");
avbotz 0:a260d84e07fc 45
avbotz 0:a260d84e07fc 46 PC.attach(&readPC);
avbotz 0:a260d84e07fc 47 IMU.attach(&readIMU);
avbotz 0:a260d84e07fc 48
avbotz 0:a260d84e07fc 49 //__disable_irq();
avbotz 0:a260d84e07fc 50 PC.printf("debug: attached interrupts.\n\r");
avbotz 0:a260d84e07fc 51
avbotz 0:a260d84e07fc 52 if (!debugMode) IMU.putc('4'); //tell the imu to start sending data, if it's not sending data already
avbotz 0:a260d84e07fc 53 PC.printf("debug: told the IMU to start sending data\n\r");
avbotz 0:a260d84e07fc 54
avbotz 0:a260d84e07fc 55 //The main loop
avbotz 0:a260d84e07fc 56 while (1) {
avbotz 0:a260d84e07fc 57 __enable_irq();
avbotz 0:a260d84e07fc 58 if (IMUreadable) {
avbotz 0:a260d84e07fc 59 myled2 = 1;
avbotz 0:a260d84e07fc 60 if (debugMode){
avbotz 0:a260d84e07fc 61 while (IMU.readable()) PC.putc(IMU.getc());
avbotz 0:a260d84e07fc 62 }
avbotz 0:a260d84e07fc 63 else {
avbotz 0:a260d84e07fc 64 while (IMU.readable()){
avbotz 0:a260d84e07fc 65 buffer.push_back(IMU.getc());
avbotz 0:a260d84e07fc 66 parseNow = (buffer.at(buffer.length() - 1) == '#');
avbotz 0:a260d84e07fc 67 }
avbotz 0:a260d84e07fc 68 }
avbotz 0:a260d84e07fc 69
avbotz 0:a260d84e07fc 70 IMUreadable = false;
avbotz 0:a260d84e07fc 71 myled2 = 0;
avbotz 0:a260d84e07fc 72 }
avbotz 0:a260d84e07fc 73 if (PCreadable) {
avbotz 0:a260d84e07fc 74 myled1 = 1;
avbotz 0:a260d84e07fc 75 while (PC.readable()) IMU.putc(PC.getc());
avbotz 0:a260d84e07fc 76 PCreadable = false;
avbotz 0:a260d84e07fc 77 myled1 = 0;
avbotz 0:a260d84e07fc 78 }
avbotz 0:a260d84e07fc 79 if (parseNow) {
avbotz 0:a260d84e07fc 80 parse(buffer);
avbotz 0:a260d84e07fc 81 buffer.clear();
avbotz 0:a260d84e07fc 82 parseNow = false;
avbotz 0:a260d84e07fc 83 }
avbotz 0:a260d84e07fc 84 }
avbotz 0:a260d84e07fc 85 }
avbotz 0:a260d84e07fc 86
avbotz 0:a260d84e07fc 87 //Interrupt called when there is a character to be read from the PC
avbotz 0:a260d84e07fc 88 //To avoid a livelock, we disable interrupts at the end of the interrupt.
avbotz 0:a260d84e07fc 89 //Then, in the main loop, we read everything from the buffer
avbotz 0:a260d84e07fc 90 void readPC(){
avbotz 0:a260d84e07fc 91 PCreadable = true;
avbotz 0:a260d84e07fc 92 __disable_irq();
avbotz 0:a260d84e07fc 93 }
avbotz 0:a260d84e07fc 94
avbotz 0:a260d84e07fc 95 //Interrupt called when there is a character to be read from the IMU
avbotz 0:a260d84e07fc 96 void readIMU(){
avbotz 0:a260d84e07fc 97 IMUreadable = true;
avbotz 0:a260d84e07fc 98 __disable_irq();
avbotz 0:a260d84e07fc 99 }
avbotz 0:a260d84e07fc 100
avbotz 0:a260d84e07fc 101 //Checks data integrity, then stores the IMU values into variables
avbotz 0:a260d84e07fc 102 void parse(string buf){
avbotz 0:a260d84e07fc 103 if (buf.find('$') != buf.npos && buf.find('#') != buf.npos){
avbotz 0:a260d84e07fc 104 for (int i = 0; i < buf.length(); i++) if (buf[i] == '$' || buf[i] == ',' || buf[i] == '#') buf[i] = ' ';
avbotz 0:a260d84e07fc 105 stringstream ss(buf);
avbotz 0:a260d84e07fc 106 ss >> accX >> accY >> accZ >> gyrX >> gyrY >> gyrZ >> magX >> magY >> magZ;
avbotz 0:a260d84e07fc 107
avbotz 0:a260d84e07fc 108 PC.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
avbotz 0:a260d84e07fc 109 }
avbotz 0:a260d84e07fc 110 else PC.printf("Bad parse!\n\r");
avbotz 0:a260d84e07fc 111 }