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@4:8f63393d49fb, 2013-07-08 (annotated)

Committer:
avbotz
Date:
Mon Jul 08 04:04:21 2013 +0000
Revision:
4:8f63393d49fb
Parent:
2:d8b182fbe018 
Initial commit.

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 2:d8b182fbe018 4 * 9dof razor from sparkfun, http://www.sparkfun.com/products/10736
avbotz 0:a260d84e07fc 5 */
avbotz 4:8f63393d49fb 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 2:d8b182fbe018 32 AnalogOut aout(p18);
avbotz 2:d8b182fbe018 33
avbotz 0:a260d84e07fc 34 int main() {
avbotz 2:d8b182fbe018 35 aout = 1.0;
avbotz 1:fdfa313b9cc3 36 // Set up the connection. Read up about parity and stop bits if this is confusing.
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 1:fdfa313b9cc3 41
avbotz 1:fdfa313b9cc3 42 for (int i = 0; i < 80; i++) {
avbotz 1:fdfa313b9cc3 43 PC.putc('-');
avbotz 1:fdfa313b9cc3 44 }
avbotz 1:fdfa313b9cc3 45 PC.printf("\n\r");
avbotz 1:fdfa313b9cc3 46
avbotz 0:a260d84e07fc 47 PC.attach(&readPC);
avbotz 0:a260d84e07fc 48 IMU.attach(&readIMU);
avbotz 1:fdfa313b9cc3 49
avbotz 1:fdfa313b9cc3 50 IMU.putc('6'); //tell the IMU to start sending data in binary mode, if it's not sending data already
avbotz 1:fdfa313b9cc3 51
avbotz 1:fdfa313b9cc3 52 //The main loop
avbotz 1:fdfa313b9cc3 53 while (true) {
avbotz 0:a260d84e07fc 54 __enable_irq();
avbotz 0:a260d84e07fc 55 if (IMUreadable) {
avbotz 0:a260d84e07fc 56 myled2 = 1;
avbotz 1:fdfa313b9cc3 57
avbotz 1:fdfa313b9cc3 58 while (IMU.readable()) {
avbotz 1:fdfa313b9cc3 59 // This snippet of code should be run whenever a character can be received from the IMU.
avbotz 1:fdfa313b9cc3 60
avbotz 1:fdfa313b9cc3 61 char data = IMU.getc();
avbotz 1:fdfa313b9cc3 62
avbotz 1:fdfa313b9cc3 63 // 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 64 if (data == '$') {
avbotz 1:fdfa313b9cc3 65 i_IMU = 0;
avbotz 1:fdfa313b9cc3 66 printf("new data\n\r");
avbotz 1:fdfa313b9cc3 67 }
avbotz 1:fdfa313b9cc3 68 // Something went wrong.
avbotz 1:fdfa313b9cc3 69 else if (i_IMU > 21) {
avbotz 1:fdfa313b9cc3 70 printf("\t\t\tIMU error.\n\r");
avbotz 1:fdfa313b9cc3 71 i_IMU = 21;
avbotz 1:fdfa313b9cc3 72 }
avbotz 1:fdfa313b9cc3 73
avbotz 1:fdfa313b9cc3 74 // End of the set of data. Parse the buffer
avbotz 2:d8b182fbe018 75 else if (i_IMU == 21) { && bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19) {
avbotz 1:fdfa313b9cc3 76 printf("Parsing\n\r");
avbotz 1:fdfa313b9cc3 77
avbotz 2:d8b182fbe018 78 //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers).
avbotz 2:d8b182fbe018 79 //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit).
avbotz 2:d8b182fbe018 80 //This turns bufIMU into a pointer to the desired value. Now we dereference it.
avbotz 2:d8b182fbe018 81 //I'm not sure if we'll still need makeCorrect. Adit, check plz <3
avbotz 2:d8b182fbe018 82 accX = *((short*)(bufIMU + 1));
avbotz 2:d8b182fbe018 83 accY = *((short*)(bufIMU + 3));
avbotz 2:d8b182fbe018 84 accZ = *((short*)(bufIMU + 5));
avbotz 2:d8b182fbe018 85
avbotz 2:d8b182fbe018 86 gyrX = *((short*)(bufIMU + 7));
avbotz 2:d8b182fbe018 87 gyrY = *((short*)(bufIMU + 9));
avbotz 2:d8b182fbe018 88 gyrZ = *((short*)(bufIMU + 11));
avbotz 2:d8b182fbe018 89
avbotz 2:d8b182fbe018 90 magX = *((short*)(bufIMU + 13));
avbotz 2:d8b182fbe018 91 magY = *((short*)(bufIMU + 15));
avbotz 2:d8b182fbe018 92 magZ = *((short*)(bufIMU + 17));
avbotz 2:d8b182fbe018 93
avbotz 1:fdfa313b9cc3 94 //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
avbotz 1:fdfa313b9cc3 95 //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
avbotz 1:fdfa313b9cc3 96 //left-shifting the first character by 8 bits and ORing it with the second character.
avbotz 2:d8b182fbe018 97
avbotz 2:d8b182fbe018 98
avbotz 2:d8b182fbe018 99 //accX = (bufIMU[1]<<8 | bufIMU[2]);
avbotz 2:d8b182fbe018 100 //accY = (bufIMU[3]<<8 | bufIMU[4]);
avbotz 2:d8b182fbe018 101 //accZ = (bufIMU[5]<<8 | bufIMU[6]);
avbotz 1:fdfa313b9cc3 102
avbotz 2:d8b182fbe018 103 //gyrX = (bufIMU[7]<<8 | bufIMU[8]);
avbotz 2:d8b182fbe018 104 //gyrY = (bufIMU[9]<<8 | bufIMU[10]);
avbotz 2:d8b182fbe018 105 //gyrZ = (bufIMU[11]<<8 | bufIMU[12]);
avbotz 1:fdfa313b9cc3 106
avbotz 2:d8b182fbe018 107 //magX = (bufIMU[13]<<8 | bufIMU[14]);
avbotz 2:d8b182fbe018 108 //magY = (bufIMU[15]<<8 | bufIMU[16]);
avbotz 2:d8b182fbe018 109 //magZ = (bufIMU[17]<<8 | bufIMU[18]);
avbotz 1:fdfa313b9cc3 110
avbotz 1:fdfa313b9cc3 111 makeCorrect(&accX);
avbotz 1:fdfa313b9cc3 112 makeCorrect(&accY);
avbotz 1:fdfa313b9cc3 113 makeCorrect(&accZ);
avbotz 1:fdfa313b9cc3 114
avbotz 1:fdfa313b9cc3 115 makeCorrect(&gyrX);
avbotz 1:fdfa313b9cc3 116 makeCorrect(&gyrY);
avbotz 1:fdfa313b9cc3 117 makeCorrect(&gyrZ);
avbotz 1:fdfa313b9cc3 118
avbotz 1:fdfa313b9cc3 119 makeCorrect(&magX);
avbotz 1:fdfa313b9cc3 120 makeCorrect(&magY);
avbotz 1:fdfa313b9cc3 121 makeCorrect(&magZ);
avbotz 1:fdfa313b9cc3 122
avbotz 1:fdfa313b9cc3 123
avbotz 1:fdfa313b9cc3 124 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 125
avbotz 1:fdfa313b9cc3 126 //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
avbotz 1:fdfa313b9cc3 127
avbotz 2:d8b182fbe018 128
avbotz 2:d8b182fbe018 129 //for (int i = 0; i < 21; i++) {
avbotz 2:d8b182fbe018 130 // PC.printf("%d ", bufIMU[i]);
avbotz 2:d8b182fbe018 131 //}
avbotz 2:d8b182fbe018 132 //PC.printf("\n\r");
avbotz 2:d8b182fbe018 133
avbotz 1:fdfa313b9cc3 134 //newIMUData = 1; // Update the flag
avbotz 1:fdfa313b9cc3 135 }
avbotz 1:fdfa313b9cc3 136
avbotz 1:fdfa313b9cc3 137
avbotz 1:fdfa313b9cc3 138 bufIMU[i_IMU] = data;
avbotz 1:fdfa313b9cc3 139 i_IMU++;
avbotz 1:fdfa313b9cc3 140 //parseNow = (buffer.at(buffer.length() - 1) == '#');
avbotz 0:a260d84e07fc 141 }
avbotz 0:a260d84e07fc 142 IMUreadable = false;
avbotz 0:a260d84e07fc 143 myled2 = 0;
avbotz 0:a260d84e07fc 144 }
avbotz 0:a260d84e07fc 145 if (PCreadable) {
avbotz 0:a260d84e07fc 146 myled1 = 1;
avbotz 0:a260d84e07fc 147 while (PC.readable()) IMU.putc(PC.getc());
avbotz 0:a260d84e07fc 148 PCreadable = false;
avbotz 0:a260d84e07fc 149 myled1 = 0;
avbotz 0:a260d84e07fc 150 }
avbotz 2:d8b182fbe018 151 //if (parseNow) {
avbotz 2:d8b182fbe018 152 // parse(buffer);
avbotz 2:d8b182fbe018 153 // buffer.clear();
avbotz 2:d8b182fbe018 154 // parseNow = false;
avbotz 2:d8b182fbe018 155 //}
avbotz 0:a260d84e07fc 156 }
avbotz 0:a260d84e07fc 157 }
avbotz 0:a260d84e07fc 158
avbotz 0:a260d84e07fc 159 //Interrupt called when there is a character to be read from the PC
avbotz 0:a260d84e07fc 160 //To avoid a livelock, we disable interrupts at the end of the interrupt.
avbotz 0:a260d84e07fc 161 //Then, in the main loop, we read everything from the buffer
avbotz 1:fdfa313b9cc3 162 void readPC() {
avbotz 0:a260d84e07fc 163 PCreadable = true;
avbotz 0:a260d84e07fc 164 __disable_irq();
avbotz 0:a260d84e07fc 165 }
avbotz 0:a260d84e07fc 166
avbotz 0:a260d84e07fc 167 //Interrupt called when there is a character to be read from the IMU
avbotz 1:fdfa313b9cc3 168 void readIMU() {
avbotz 0:a260d84e07fc 169 IMUreadable = true;
avbotz 0:a260d84e07fc 170 __disable_irq();
avbotz 0:a260d84e07fc 171 }
avbotz 0:a260d84e07fc 172
avbotz 0:a260d84e07fc 173
avbotz 2:d8b182fbe018 174 //So negative numbers that are transferred are in twos complement form
avbotz 2:d8b182fbe018 175 // and the compiler seems to like to use things created by bitwise operators
avbotz 2:d8b182fbe018 176 // in unsigned form so all of the bits are switched and we switch it back and center
avbotz 2:d8b182fbe018 177 // it around 512 which we are using as out zero value
avbotz 1:fdfa313b9cc3 178 inline void makeCorrect (short* i) {
avbotz 1:fdfa313b9cc3 179 if ((*i)>>15) *i = 512 - (~(*i));
avbotz 1:fdfa313b9cc3 180 else *i = 512 + *i;
avbotz 2:d8b182fbe018 181 }