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@1:fdfa313b9cc3, 2011-10-17 (annotated)

Committer:
avbotz
Date:
Mon Oct 17 15:19:01 2011 +0000
Revision:
1:fdfa313b9cc3
Parent:
0:a260d84e07fc
Child:
2:d8b182fbe018
IMU parsing code that utilizes the IMU\'s binary mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
avbotz 1:fdfa313b9cc3 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 1:fdfa313b9cc3 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
avbotz 0:a260d84e07fc 11 Serial IMU(p9, p10); // tx, rx
avbotz 0:a260d84e07fc 12 Serial PC(USBTX, USBRX);
avbotz 0:a260d84e07fc 13
avbotz 0:a260d84e07fc 14 DigitalOut myled1(LED1);
avbotz 0:a260d84e07fc 15 DigitalOut myled2(LED2);
avbotz 0:a260d84e07fc 16 DigitalOut myled3(LED3);
avbotz 0:a260d84e07fc 17 DigitalOut myled4(LED4);
avbotz 0:a260d84e07fc 18
avbotz 0:a260d84e07fc 19 bool IMUreadable = false;
avbotz 0:a260d84e07fc 20 bool PCreadable = false;
avbotz 0:a260d84e07fc 21
avbotz 0:a260d84e07fc 22 void readIMU();
avbotz 0:a260d84e07fc 23 void readPC();
avbotz 1:fdfa313b9cc3 24 inline void makeCorrect(short* i);
avbotz 0:a260d84e07fc 25
avbotz 1:fdfa313b9cc3 26 short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;
avbotz 0:a260d84e07fc 27
avbotz 1:fdfa313b9cc3 28 // Expected format: {$, accX, accX, accY, accY, accZ, accZ, gyrX, gyrX, gyrY, gyrY, gyrZ, gyrZ, magX, magX, magY, magY, magZ, magZ, #, \r}
avbotz 1:fdfa313b9cc3 29 int i_IMU;
avbotz 1:fdfa313b9cc3 30 char bufIMU[21];
avbotz 0:a260d84e07fc 31
avbotz 0:a260d84e07fc 32 int main() {
avbotz 1:fdfa313b9cc3 33 // Set up the connection. Read up about parity and stop bits if this is confusing.
avbotz 0:a260d84e07fc 34 IMU.format(8, Serial::None, 1);
avbotz 0:a260d84e07fc 35 PC.format(8, Serial::None, 1);
avbotz 0:a260d84e07fc 36 IMU.baud(57600);
avbotz 0:a260d84e07fc 37 PC.baud(115200);
avbotz 1:fdfa313b9cc3 38
avbotz 1:fdfa313b9cc3 39 for (int i = 0; i < 80; i++) {
avbotz 1:fdfa313b9cc3 40 PC.putc('-');
avbotz 1:fdfa313b9cc3 41 }
avbotz 1:fdfa313b9cc3 42 PC.printf("\n\r");
avbotz 1:fdfa313b9cc3 43
avbotz 0:a260d84e07fc 44 PC.attach(&readPC);
avbotz 0:a260d84e07fc 45 IMU.attach(&readIMU);
avbotz 1:fdfa313b9cc3 46
avbotz 1:fdfa313b9cc3 47 IMU.putc('6'); //tell the IMU to start sending data in binary mode, if it's not sending data already
avbotz 1:fdfa313b9cc3 48
avbotz 1:fdfa313b9cc3 49 //The main loop
avbotz 1:fdfa313b9cc3 50 while (true) {
avbotz 0:a260d84e07fc 51 __enable_irq();
avbotz 0:a260d84e07fc 52 if (IMUreadable) {
avbotz 0:a260d84e07fc 53 myled2 = 1;
avbotz 1:fdfa313b9cc3 54
avbotz 1:fdfa313b9cc3 55 while (IMU.readable()) {
avbotz 1:fdfa313b9cc3 56 // This snippet of code should be run whenever a character can be received from the IMU.
avbotz 1:fdfa313b9cc3 57
avbotz 1:fdfa313b9cc3 58 char data = IMU.getc();
avbotz 1:fdfa313b9cc3 59
avbotz 1:fdfa313b9cc3 60 // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there.
avbotz 1:fdfa313b9cc3 61 if (data == '$') {
avbotz 1:fdfa313b9cc3 62 i_IMU = 0;
avbotz 1:fdfa313b9cc3 63 printf("new data\n\r");
avbotz 1:fdfa313b9cc3 64 }
avbotz 1:fdfa313b9cc3 65 // Something went wrong.
avbotz 1:fdfa313b9cc3 66 else if (i_IMU > 21) {
avbotz 1:fdfa313b9cc3 67 printf("\t\t\tIMU error.\n\r");
avbotz 1:fdfa313b9cc3 68 i_IMU = 21;
avbotz 1:fdfa313b9cc3 69 }
avbotz 1:fdfa313b9cc3 70
avbotz 1:fdfa313b9cc3 71 // End of the set of data. Parse the buffer
avbotz 1:fdfa313b9cc3 72 else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
avbotz 1:fdfa313b9cc3 73 printf("Parsing\n\r");
avbotz 1:fdfa313b9cc3 74
avbotz 1:fdfa313b9cc3 75 //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
avbotz 1:fdfa313b9cc3 76 //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
avbotz 1:fdfa313b9cc3 77 //left-shifting the first character by 8 bits and ORing it with the second character.
avbotz 1:fdfa313b9cc3 78 accX = (bufIMU[1]<<8 | bufIMU[2]);
avbotz 1:fdfa313b9cc3 79 accY = (bufIMU[3]<<8 | bufIMU[4]);
avbotz 1:fdfa313b9cc3 80 accZ = (bufIMU[5]<<8 | bufIMU[6]);
avbotz 1:fdfa313b9cc3 81
avbotz 1:fdfa313b9cc3 82 gyrX = (bufIMU[7]<<8 | bufIMU[8]);
avbotz 1:fdfa313b9cc3 83 gyrY = (bufIMU[9]<<8 | bufIMU[10]);
avbotz 1:fdfa313b9cc3 84 gyrZ = (bufIMU[11]<<8 | bufIMU[12]);
avbotz 1:fdfa313b9cc3 85
avbotz 1:fdfa313b9cc3 86 magX = (bufIMU[13]<<8 | bufIMU[14]);
avbotz 1:fdfa313b9cc3 87 magY = (bufIMU[15]<<8 | bufIMU[16]);
avbotz 1:fdfa313b9cc3 88 magZ = (bufIMU[17]<<8 | bufIMU[18]);
avbotz 1:fdfa313b9cc3 89
avbotz 1:fdfa313b9cc3 90 makeCorrect(&accX);
avbotz 1:fdfa313b9cc3 91 makeCorrect(&accY);
avbotz 1:fdfa313b9cc3 92 makeCorrect(&accZ);
avbotz 1:fdfa313b9cc3 93
avbotz 1:fdfa313b9cc3 94 makeCorrect(&gyrX);
avbotz 1:fdfa313b9cc3 95 makeCorrect(&gyrY);
avbotz 1:fdfa313b9cc3 96 makeCorrect(&gyrZ);
avbotz 1:fdfa313b9cc3 97
avbotz 1:fdfa313b9cc3 98 makeCorrect(&magX);
avbotz 1:fdfa313b9cc3 99 makeCorrect(&magY);
avbotz 1:fdfa313b9cc3 100 makeCorrect(&magZ);
avbotz 1:fdfa313b9cc3 101
avbotz 1:fdfa313b9cc3 102
avbotz 1:fdfa313b9cc3 103 PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);
avbotz 1:fdfa313b9cc3 104
avbotz 1:fdfa313b9cc3 105 //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
avbotz 1:fdfa313b9cc3 106
avbotz 1:fdfa313b9cc3 107 /*
avbotz 1:fdfa313b9cc3 108 for (int i = 0; i < 21; i++) {
avbotz 1:fdfa313b9cc3 109 PC.printf("%d ", bufIMU[i]);
avbotz 1:fdfa313b9cc3 110 }
avbotz 1:fdfa313b9cc3 111 PC.printf("\n\r");
avbotz 1:fdfa313b9cc3 112 */
avbotz 1:fdfa313b9cc3 113 //newIMUData = 1; // Update the flag
avbotz 1:fdfa313b9cc3 114 }
avbotz 1:fdfa313b9cc3 115
avbotz 1:fdfa313b9cc3 116
avbotz 1:fdfa313b9cc3 117 bufIMU[i_IMU] = data;
avbotz 1:fdfa313b9cc3 118 i_IMU++;
avbotz 1:fdfa313b9cc3 119 //parseNow = (buffer.at(buffer.length() - 1) == '#');
avbotz 0:a260d84e07fc 120 }
avbotz 0:a260d84e07fc 121 IMUreadable = false;
avbotz 0:a260d84e07fc 122 myled2 = 0;
avbotz 0:a260d84e07fc 123 }
avbotz 0:a260d84e07fc 124 if (PCreadable) {
avbotz 0:a260d84e07fc 125 myled1 = 1;
avbotz 0:a260d84e07fc 126 while (PC.readable()) IMU.putc(PC.getc());
avbotz 0:a260d84e07fc 127 PCreadable = false;
avbotz 0:a260d84e07fc 128 myled1 = 0;
avbotz 0:a260d84e07fc 129 }
avbotz 1:fdfa313b9cc3 130 /*if (parseNow) {
avbotz 0:a260d84e07fc 131 parse(buffer);
avbotz 0:a260d84e07fc 132 buffer.clear();
avbotz 0:a260d84e07fc 133 parseNow = false;
avbotz 1:fdfa313b9cc3 134 }*/
avbotz 0:a260d84e07fc 135 }
avbotz 0:a260d84e07fc 136 }
avbotz 0:a260d84e07fc 137
avbotz 0:a260d84e07fc 138 //Interrupt called when there is a character to be read from the PC
avbotz 0:a260d84e07fc 139 //To avoid a livelock, we disable interrupts at the end of the interrupt.
avbotz 0:a260d84e07fc 140 //Then, in the main loop, we read everything from the buffer
avbotz 1:fdfa313b9cc3 141 void readPC() {
avbotz 0:a260d84e07fc 142 PCreadable = true;
avbotz 0:a260d84e07fc 143 __disable_irq();
avbotz 0:a260d84e07fc 144 }
avbotz 0:a260d84e07fc 145
avbotz 0:a260d84e07fc 146 //Interrupt called when there is a character to be read from the IMU
avbotz 1:fdfa313b9cc3 147 void readIMU() {
avbotz 0:a260d84e07fc 148 IMUreadable = true;
avbotz 0:a260d84e07fc 149 __disable_irq();
avbotz 0:a260d84e07fc 150 }
avbotz 0:a260d84e07fc 151
avbotz 0:a260d84e07fc 152
avbotz 1:fdfa313b9cc3 153 //The bitshift performed always creates a positive integer. Sometimes, the IMU
avbotz 1:fdfa313b9cc3 154 //values are negative. This fixes that - it centers the value around 512. That is,
avbotz 1:fdfa313b9cc3 155 //512 is 0 for the IMU variables. Values above it are positive, and values below it are negative.
avbotz 1:fdfa313b9cc3 156 inline void makeCorrect (short* i) {
avbotz 1:fdfa313b9cc3 157 if ((*i)>>15) *i = 512 - (~(*i));
avbotz 1:fdfa313b9cc3 158 else *i = 512 + *i;
avbotz 1:fdfa313b9cc3 159 }