Mitchell Pang / Mbed 2 deprecated RC_boat

RC Boat using a C# GUI over XBee communication.

Dependencies:   MODSERIAL LSM9DS0 Motordriver PID mbed-rtos mbed

RC Boat Wiki Page

main.cpp@4:6f6a9602e92d, 2015-04-29 (annotated)

Committer:
admcrae
Date:
Wed Apr 29 17:27:08 2015 +0000
Revision:
4:6f6a9602e92d
Parent:
0:58395e885409
Child:
6:c0a2ac7a8c26
Mostly working version.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mitchpang 0:58395e885409 1 #include "mbed.h"
mitchpang 0:58395e885409 2 #include "LSM9DS0.h"
mitchpang 0:58395e885409 3 #include "rtos.h"
mitchpang 0:58395e885409 4 #include "PID.h"
mitchpang 0:58395e885409 5 #include "motordriver.h"
admcrae 4:6f6a9602e92d 6 #include "MODSERIAL.h"
mitchpang 0:58395e885409 7 //#include "IMUfilter.h"
mitchpang 0:58395e885409 8
mitchpang 0:58395e885409 9 //Gravity at Earth's surface in m/s/s
mitchpang 0:58395e885409 10 #define g0 9.812865328
mitchpang 0:58395e885409 11 //Number of samples to average.
mitchpang 0:58395e885409 12 #define SAMPLES 4
mitchpang 0:58395e885409 13 //Number of samples to be averaged for a null bias calculation
mitchpang 0:58395e885409 14 //during calibration.
mitchpang 0:58395e885409 15 #define CALIBRATION_SAMPLES 1024
mitchpang 0:58395e885409 16 //Convert from radians to degrees.
mitchpang 0:58395e885409 17 #define toDegrees(x) (x * 57.2957795)
mitchpang 0:58395e885409 18 //Convert from degrees to radians.
mitchpang 0:58395e885409 19 #define toRadians(x) (x * 0.01745329252)
mitchpang 0:58395e885409 20 //LSM9DS0 gyroscope sensitivity is 8.75 (millidegrees/sec)/digit when set to +-2G
mitchpang 0:58395e885409 21 #define GYROSCOPE_GAIN (0.007476806640625)
mitchpang 0:58395e885409 22 //Full scale resolution on the accelerometer is 0.0001220703125g/LSB.
mitchpang 0:58395e885409 23 #define ACCELEROMETER_GAIN (0.00006103515625 * g0)
mitchpang 0:58395e885409 24 //Sampling gyroscope at 100Hz.
mitchpang 0:58395e885409 25 #define GYRO_RATE 0.01
mitchpang 0:58395e885409 26 //Sampling accelerometer at 200Hz.
mitchpang 0:58395e885409 27 #define ACC_RATE 0.005
mitchpang 0:58395e885409 28 // SDO_XM and SDO_G are both grounded, so our addresses are:
mitchpang 0:58395e885409 29 #define LSM9DS0_XM 0x1E // Would be 0x1E if SDO_XM is LOW
mitchpang 0:58395e885409 30 #define LSM9DS0_G 0x6A // Would be 0x6A if SDO_G is LOW
admcrae 4:6f6a9602e92d 31 #define RATE 0.05 // Define rate for PID loop
mitchpang 0:58395e885409 32
mitchpang 0:58395e885409 33
mitchpang 0:58395e885409 34 Serial pc(USBTX, USBRX);
mitchpang 0:58395e885409 35 LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
mitchpang 0:58395e885409 36 Mutex pcMutex;
admcrae 4:6f6a9602e92d 37 MODSERIAL xbee(p13, p14, 64, 128);
mitchpang 0:58395e885409 38 PID linearController(1.0, 0.0, 0.0, RATE);
mitchpang 0:58395e885409 39 PID angularController(1.0, 0.0, 0.0, RATE);
mitchpang 0:58395e885409 40 DigitalOut led1(LED1);
mitchpang 0:58395e885409 41 DigitalOut led2(LED2);
mitchpang 0:58395e885409 42 DigitalOut led3(LED3);
mitchpang 0:58395e885409 43 DigitalOut led4(LED4);
admcrae 4:6f6a9602e92d 44 int usePID = 1;
admcrae 4:6f6a9602e92d 45 int commReceived = 0;
mitchpang 0:58395e885409 46
mitchpang 0:58395e885409 47 //Gloabal Variables for comm system below
mitchpang 0:58395e885409 48 Ticker commTicker;
mitchpang 0:58395e885409 49
mitchpang 0:58395e885409 50
mitchpang 0:58395e885409 51 //Global Variables for motor control below
mitchpang 0:58395e885409 52 Motor leftMotor(p21, p22, p23, 1); // pwm, fwd, rev
mitchpang 0:58395e885409 53 Motor rightMotor(p26, p25, p24, 1); // pwm, fwd, rev
mitchpang 0:58395e885409 54
mitchpang 0:58395e885409 55 //Offsets for the gyroscope.
mitchpang 0:58395e885409 56 //The readings we take when the gyroscope is stationary won't be 0, so we'll
mitchpang 0:58395e885409 57 //average a set of readings we do get when the gyroscope is stationary and
mitchpang 0:58395e885409 58 //take those away from subsequent readings to ensure the gyroscope is offset
mitchpang 0:58395e885409 59 //or "biased" to 0.
mitchpang 0:58395e885409 60 double w_xBias;
mitchpang 0:58395e885409 61 double w_yBias;
mitchpang 0:58395e885409 62 double w_zBias;
mitchpang 0:58395e885409 63
mitchpang 0:58395e885409 64 //Offsets for the accelerometer.
mitchpang 0:58395e885409 65 //Same as with the gyroscope.
mitchpang 0:58395e885409 66 double a_xBias;
mitchpang 0:58395e885409 67 double a_yBias;
mitchpang 0:58395e885409 68 double a_zBias;
mitchpang 0:58395e885409 69
mitchpang 0:58395e885409 70 //Accumulators used for oversampling and then averaging.
mitchpang 0:58395e885409 71 double a_xAccumulator = 0;
mitchpang 0:58395e885409 72 double a_yAccumulator = 0;
mitchpang 0:58395e885409 73 double a_zAccumulator = 0;
mitchpang 0:58395e885409 74 double w_xAccumulator = 0;
mitchpang 0:58395e885409 75 double w_yAccumulator = 0;
mitchpang 0:58395e885409 76 double w_zAccumulator = 0;
mitchpang 0:58395e885409 77
mitchpang 0:58395e885409 78 //Accelerometer and gyroscope readings for x, y, z axes.
mitchpang 0:58395e885409 79 double a_x;
mitchpang 0:58395e885409 80 double a_y;
mitchpang 0:58395e885409 81 double a_z;
mitchpang 0:58395e885409 82 double w_x;
mitchpang 0:58395e885409 83 double w_y;
mitchpang 0:58395e885409 84 double w_z;
mitchpang 0:58395e885409 85
mitchpang 0:58395e885409 86 //Previous Acceleromerter and gyroscope readings for integration
mitchpang 0:58395e885409 87 double last_a_x;
mitchpang 0:58395e885409 88 double last_a_y;
mitchpang 0:58395e885409 89 double last_a_z;
mitchpang 0:58395e885409 90
mitchpang 0:58395e885409 91 //Buffer for accelerometer readings.
mitchpang 0:58395e885409 92 int readings[3];
mitchpang 0:58395e885409 93 //Number of accelerometer samples we're on.
mitchpang 0:58395e885409 94 int accelerometerSamples = 0;
mitchpang 0:58395e885409 95 //Number of gyroscope samples we're on.
mitchpang 0:58395e885409 96 int gyroscopeSamples = 0;
mitchpang 0:58395e885409 97
mitchpang 0:58395e885409 98 //current readings of linear velocity
mitchpang 0:58395e885409 99 double v_x = 0;
mitchpang 0:58395e885409 100 double v_y = 0;
mitchpang 0:58395e885409 101 double v_z = 0;
mitchpang 0:58395e885409 102 double v_mag2 = 0; // linear velocity squared. sqrt() is comupationally difficult
mitchpang 0:58395e885409 103
admcrae 4:6f6a9602e92d 104 /*
mitchpang 0:58395e885409 105 //values for pwm wave to motors
mitchpang 0:58395e885409 106 float linearOutput = 0;
mitchpang 0:58395e885409 107 float angularOutput = 0;
admcrae 4:6f6a9602e92d 108 */
admcrae 4:6f6a9602e92d 109
admcrae 4:6f6a9602e92d 110 //setpoints from xbee
admcrae 4:6f6a9602e92d 111 float setLinear = 0;
admcrae 4:6f6a9602e92d 112 float setAngular = 0;
admcrae 4:6f6a9602e92d 113 int enabled = 0;
admcrae 4:6f6a9602e92d 114
admcrae 4:6f6a9602e92d 115
mitchpang 0:58395e885409 116 /**
mitchpang 0:58395e885409 117 * Prototypes
mitchpang 0:58395e885409 118 */
mitchpang 0:58395e885409 119 //Calculate the null bias.
mitchpang 0:58395e885409 120 void calibrateAccelerometer(void);
mitchpang 0:58395e885409 121 //Take a set of samples and average them.
mitchpang 0:58395e885409 122 void sampleAccelerometer(void);
mitchpang 0:58395e885409 123 //Calculate the null bias.
mitchpang 0:58395e885409 124 void calibrateGyroscope(void);
mitchpang 0:58395e885409 125 //Take a set of samples and average them.
mitchpang 0:58395e885409 126 void sampleGyroscope(void);
mitchpang 0:58395e885409 127
mitchpang 0:58395e885409 128 void startPID() {
mitchpang 0:58395e885409 129 linearController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 130 angularController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 131 usePID = 1;
admcrae 4:6f6a9602e92d 132 }
mitchpang 0:58395e885409 133
mitchpang 0:58395e885409 134 void stopPID() {
mitchpang 0:58395e885409 135 linearController.setMode(MANUAL_MODE);
mitchpang 0:58395e885409 136 angularController.setMode(MANUAL_MODE);
mitchpang 0:58395e885409 137 usePID = 0;
admcrae 4:6f6a9602e92d 138 }
admcrae 4:6f6a9602e92d 139
admcrae 4:6f6a9602e92d 140 //Variables for storing commands
admcrae 4:6f6a9602e92d 141 char commBuffer[2][64];
admcrae 4:6f6a9602e92d 142 int currentBuff = 0;
admcrae 4:6f6a9602e92d 143
admcrae 4:6f6a9602e92d 144 void parseCommand() {
admcrae 4:6f6a9602e92d 145 float tempForward, tempAngular;
admcrae 4:6f6a9602e92d 146 int tempEnabled, tempPID;
admcrae 4:6f6a9602e92d 147 sscanf(commBuffer[currentBuff], "F%f A%f E%d P%d\r", &tempForward, &tempAngular, &tempEnabled, &tempPID);
admcrae 4:6f6a9602e92d 148
admcrae 4:6f6a9602e92d 149 //Change global variables
admcrae 4:6f6a9602e92d 150 setLinear = tempForward;
admcrae 4:6f6a9602e92d 151 setAngular = tempAngular;
admcrae 4:6f6a9602e92d 152 enabled = tempEnabled;
admcrae 4:6f6a9602e92d 153
admcrae 4:6f6a9602e92d 154 if (tempPID) {
admcrae 4:6f6a9602e92d 155 startPID();
admcrae 4:6f6a9602e92d 156 } else {
admcrae 4:6f6a9602e92d 157 stopPID();
mitchpang 0:58395e885409 158 }
admcrae 4:6f6a9602e92d 159
admcrae 4:6f6a9602e92d 160 }
admcrae 4:6f6a9602e92d 161
admcrae 4:6f6a9602e92d 162 void parseInput(void const *args) {
admcrae 4:6f6a9602e92d 163 static int i = 0;
admcrae 4:6f6a9602e92d 164 while (1) {
admcrae 4:6f6a9602e92d 165 while (xbee.readable()) {
admcrae 4:6f6a9602e92d 166 commBuffer[1 - currentBuff][i] = xbee.getc();
admcrae 4:6f6a9602e92d 167 if (commBuffer[1 - currentBuff][i] == '\r') {
admcrae 4:6f6a9602e92d 168 commBuffer[1 - currentBuff][i+1] = '\0';
admcrae 4:6f6a9602e92d 169 i = 0;
admcrae 4:6f6a9602e92d 170 currentBuff = 1 - currentBuff;
admcrae 4:6f6a9602e92d 171 parseCommand();
admcrae 4:6f6a9602e92d 172 commReceived = 1;
admcrae 4:6f6a9602e92d 173 } else {
admcrae 4:6f6a9602e92d 174 ++i;
admcrae 4:6f6a9602e92d 175 }
admcrae 4:6f6a9602e92d 176 }
admcrae 4:6f6a9602e92d 177 Thread::wait(50);
admcrae 4:6f6a9602e92d 178 }
admcrae 4:6f6a9602e92d 179 }
admcrae 4:6f6a9602e92d 180
mitchpang 0:58395e885409 181
mitchpang 0:58395e885409 182 void checkCOMRecieved() {
admcrae 4:6f6a9602e92d 183 if (commReceived == 0) {
mitchpang 0:58395e885409 184 stopPID();
admcrae 4:6f6a9602e92d 185 enabled = 0;
mitchpang 0:58395e885409 186 led1 = led2 = led3 = led4 = 1;
admcrae 4:6f6a9602e92d 187 } else {
admcrae 4:6f6a9602e92d 188 led1 = led2 = led3 = led4 = 0;
mitchpang 0:58395e885409 189 }
admcrae 4:6f6a9602e92d 190 commReceived = 0;
mitchpang 0:58395e885409 191 }
mitchpang 0:58395e885409 192
mitchpang 0:58395e885409 193 void sampleAccelerometer(void const *args) {
mitchpang 0:58395e885409 194 while(1) {
mitchpang 0:58395e885409 195 while(usePID != 1) {
mitchpang 0:58395e885409 196 Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
mitchpang 0:58395e885409 197 }
mitchpang 0:58395e885409 198 //Have we taken enough samples?
mitchpang 0:58395e885409 199 if (accelerometerSamples == SAMPLES) {
mitchpang 0:58395e885409 200
mitchpang 0:58395e885409 201 //Average the samples, remove the bias, and calculate the acceleration
mitchpang 0:58395e885409 202 //in m/s/s.
mitchpang 0:58395e885409 203 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 204 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 205 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
mitchpang 0:58395e885409 206
mitchpang 0:58395e885409 207 a_xAccumulator = 0;
mitchpang 0:58395e885409 208 a_yAccumulator = 0;
mitchpang 0:58395e885409 209 a_zAccumulator = 0;
mitchpang 0:58395e885409 210 accelerometerSamples = 0;
mitchpang 0:58395e885409 211 pcMutex.lock();
mitchpang 0:58395e885409 212 pc.printf("a_x: %f a_y: %f a_z: %f \n",a_x,a_y,a_z);
mitchpang 0:58395e885409 213 pcMutex.unlock();
mitchpang 0:58395e885409 214
mitchpang 0:58395e885409 215 // integrate to get velocity. make sure to subtract gravity downwards!
mitchpang 0:58395e885409 216
mitchpang 0:58395e885409 217 v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 218 v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 219 v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
mitchpang 0:58395e885409 220
mitchpang 0:58395e885409 221 last_a_x = a_x;
mitchpang 0:58395e885409 222 last_a_y = a_y;
mitchpang 0:58395e885409 223 last_a_z = a_z;
mitchpang 0:58395e885409 224 pcMutex.lock();
mitchpang 0:58395e885409 225 pc.printf("v_x: %f v_y: %f v_z: %f \n",v_x,v_y,v_z);
mitchpang 0:58395e885409 226 pcMutex.unlock();
mitchpang 0:58395e885409 227
mitchpang 0:58395e885409 228 } else {
mitchpang 0:58395e885409 229 //Take another sample.
mitchpang 0:58395e885409 230 imu.readAccel();
mitchpang 0:58395e885409 231 pcMutex.lock();
mitchpang 0:58395e885409 232 pc.printf("A: ");
mitchpang 0:58395e885409 233 pc.printf("%d", imu.ax);
mitchpang 0:58395e885409 234 pc.printf(", ");
mitchpang 0:58395e885409 235 pc.printf("%d",imu.ay);
mitchpang 0:58395e885409 236 pc.printf(", ");
mitchpang 0:58395e885409 237 pc.printf("%d\n",imu.az);
mitchpang 0:58395e885409 238 pcMutex.unlock();
mitchpang 0:58395e885409 239
mitchpang 0:58395e885409 240 a_xAccumulator += imu.ax;
mitchpang 0:58395e885409 241 a_yAccumulator += imu.ay;
mitchpang 0:58395e885409 242 a_zAccumulator += imu.az;
mitchpang 0:58395e885409 243
mitchpang 0:58395e885409 244 accelerometerSamples++;
mitchpang 0:58395e885409 245 Thread::wait(ACC_RATE * 1000);
mitchpang 0:58395e885409 246 }
mitchpang 0:58395e885409 247
mitchpang 0:58395e885409 248
mitchpang 0:58395e885409 249 }
mitchpang 0:58395e885409 250 }
mitchpang 0:58395e885409 251
mitchpang 0:58395e885409 252 void calibrateAccelerometer(void) {
mitchpang 0:58395e885409 253
mitchpang 0:58395e885409 254 led1 = 1;
mitchpang 0:58395e885409 255
mitchpang 0:58395e885409 256 a_xAccumulator = 0;
mitchpang 0:58395e885409 257 a_yAccumulator = 0;
mitchpang 0:58395e885409 258 a_zAccumulator = 0;
mitchpang 0:58395e885409 259
mitchpang 0:58395e885409 260
mitchpang 0:58395e885409 261 //Take a number of readings and average them
mitchpang 0:58395e885409 262 //to calculate the zero g offset.
mitchpang 0:58395e885409 263 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 0:58395e885409 264
mitchpang 0:58395e885409 265 imu.readAccel();
mitchpang 0:58395e885409 266
mitchpang 0:58395e885409 267 a_xAccumulator += (double) imu.ax;
mitchpang 0:58395e885409 268 a_yAccumulator += (double) imu.ay;
mitchpang 0:58395e885409 269 a_zAccumulator += (double) imu.az;
mitchpang 0:58395e885409 270
mitchpang 0:58395e885409 271 wait(ACC_RATE);
mitchpang 0:58395e885409 272
mitchpang 0:58395e885409 273 }
mitchpang 0:58395e885409 274
mitchpang 0:58395e885409 275 a_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 276 a_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 277 a_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 278
mitchpang 0:58395e885409 279 //At 4mg/LSB, 250 LSBs is 1g.
mitchpang 0:58395e885409 280 a_xBias = a_xAccumulator;
mitchpang 0:58395e885409 281 a_yBias = a_yAccumulator;
mitchpang 0:58395e885409 282 //a_zBias = (a_zAccumulator - (1/0.00006103515625));
mitchpang 0:58395e885409 283 a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
mitchpang 0:58395e885409 284
mitchpang 0:58395e885409 285 pcMutex.lock();
mitchpang 0:58395e885409 286 pc.printf("A_Bias: ");
mitchpang 0:58395e885409 287 pc.printf("%f", a_xBias);
mitchpang 0:58395e885409 288 pc.printf(", ");
mitchpang 0:58395e885409 289 pc.printf("%f",a_yBias);
mitchpang 0:58395e885409 290 pc.printf(", ");
mitchpang 0:58395e885409 291 pc.printf("%f\n",a_zBias);
mitchpang 0:58395e885409 292 pcMutex.unlock();
mitchpang 0:58395e885409 293
mitchpang 0:58395e885409 294 a_xAccumulator = 0;
mitchpang 0:58395e885409 295 a_yAccumulator = 0;
mitchpang 0:58395e885409 296 a_zAccumulator = 0;
mitchpang 0:58395e885409 297 pc.printf("done calibrating accel\n");
mitchpang 0:58395e885409 298 led1 = 0;
mitchpang 0:58395e885409 299 }
mitchpang 0:58395e885409 300
mitchpang 0:58395e885409 301
mitchpang 0:58395e885409 302 void calibrateGyroscope(void) {
mitchpang 0:58395e885409 303 led2 = 1;
mitchpang 0:58395e885409 304 w_xAccumulator = 0;
mitchpang 0:58395e885409 305 w_yAccumulator = 0;
mitchpang 0:58395e885409 306 w_zAccumulator = 0;
mitchpang 0:58395e885409 307
mitchpang 0:58395e885409 308 //Take a number of readings and average them
mitchpang 0:58395e885409 309 //to calculate the gyroscope bias offset.
mitchpang 0:58395e885409 310 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 0:58395e885409 311 imu.readGyro();
mitchpang 0:58395e885409 312 w_xAccumulator += (double) imu.gx;
mitchpang 0:58395e885409 313 w_yAccumulator += (double) imu.gy;
mitchpang 0:58395e885409 314 w_zAccumulator += (double) imu.gz;
mitchpang 0:58395e885409 315 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 316
mitchpang 0:58395e885409 317 }
mitchpang 0:58395e885409 318
mitchpang 0:58395e885409 319 //Average the samples.
mitchpang 0:58395e885409 320 w_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 321 w_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 322 w_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 323
mitchpang 0:58395e885409 324 w_xBias = w_xAccumulator;
mitchpang 0:58395e885409 325 w_yBias = w_yAccumulator;
mitchpang 0:58395e885409 326 w_zBias = w_zAccumulator;
mitchpang 0:58395e885409 327
mitchpang 0:58395e885409 328 pcMutex.lock();
mitchpang 0:58395e885409 329 pc.printf("G_Bias: ");
mitchpang 0:58395e885409 330 pc.printf("%f", w_xBias);
mitchpang 0:58395e885409 331 pc.printf(", ");
mitchpang 0:58395e885409 332 pc.printf("%f",w_yBias);
mitchpang 0:58395e885409 333 pc.printf(", ");
mitchpang 0:58395e885409 334 pc.printf("%f\n",w_zBias);
mitchpang 0:58395e885409 335 pcMutex.unlock();
mitchpang 0:58395e885409 336
mitchpang 0:58395e885409 337 w_xAccumulator = 0;
mitchpang 0:58395e885409 338 w_yAccumulator = 0;
mitchpang 0:58395e885409 339 w_zAccumulator = 0;
mitchpang 0:58395e885409 340 pc.printf("done calibrating gyro\n");
mitchpang 0:58395e885409 341 led2 = 0;
mitchpang 0:58395e885409 342 }
mitchpang 0:58395e885409 343
mitchpang 0:58395e885409 344 void sampleGyroscope(void const *args) {
mitchpang 0:58395e885409 345 while(1){
mitchpang 0:58395e885409 346 while(usePID != 1) {
mitchpang 0:58395e885409 347 Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
mitchpang 0:58395e885409 348 }
mitchpang 0:58395e885409 349 //Have we taken enough samples?
mitchpang 0:58395e885409 350 if (gyroscopeSamples == SAMPLES) {
mitchpang 0:58395e885409 351
mitchpang 0:58395e885409 352 //Average the samples, remove the bias, and calculate the angular
mitchpang 0:58395e885409 353 //velocity in deg/s.
mitchpang 0:58395e885409 354 w_x = ((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 355 w_y = ((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 356 w_z = ((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN;
mitchpang 0:58395e885409 357 pcMutex.lock();
mitchpang 0:58395e885409 358 pc.printf("w_x: %f w_y: %f w_z: %f \n",w_x,w_y,w_z);
mitchpang 0:58395e885409 359 pcMutex.unlock();
mitchpang 0:58395e885409 360 w_xAccumulator = 0;
mitchpang 0:58395e885409 361 w_yAccumulator = 0;
mitchpang 0:58395e885409 362 w_zAccumulator = 0;
mitchpang 0:58395e885409 363 gyroscopeSamples = 0;
mitchpang 0:58395e885409 364
mitchpang 0:58395e885409 365 } else {
mitchpang 0:58395e885409 366 imu.readGyro();
mitchpang 0:58395e885409 367 pcMutex.lock();
mitchpang 0:58395e885409 368 pc.printf("G: ");
mitchpang 0:58395e885409 369 pc.printf("%d", imu.gx);
mitchpang 0:58395e885409 370 pc.printf(", ");
mitchpang 0:58395e885409 371 pc.printf("%d",imu.gy);
mitchpang 0:58395e885409 372 pc.printf(", ");
mitchpang 0:58395e885409 373 pc.printf("%d\n",imu.gz);
mitchpang 0:58395e885409 374 pcMutex.unlock();
mitchpang 0:58395e885409 375
mitchpang 0:58395e885409 376 w_xAccumulator += imu.gx;
mitchpang 0:58395e885409 377 w_yAccumulator += imu.gy;
mitchpang 0:58395e885409 378 w_zAccumulator += imu.gz;
mitchpang 0:58395e885409 379
mitchpang 0:58395e885409 380 gyroscopeSamples++;
mitchpang 0:58395e885409 381 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 382 }
mitchpang 0:58395e885409 383
mitchpang 0:58395e885409 384 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 385 }
mitchpang 0:58395e885409 386 }
mitchpang 0:58395e885409 387
mitchpang 0:58395e885409 388
mitchpang 0:58395e885409 389
mitchpang 0:58395e885409 390
mitchpang 0:58395e885409 391 void setup()
mitchpang 0:58395e885409 392 {
mitchpang 0:58395e885409 393 pc.baud(9600); // Start serial at 115200 bps
mitchpang 0:58395e885409 394 // Use the begin() function to initialize the LSM9DS0 library.
mitchpang 0:58395e885409 395 // You can either call it with no parameters (the easy way):
mitchpang 0:58395e885409 396 //uint16_t status = dof.begin();
mitchpang 0:58395e885409 397
mitchpang 0:58395e885409 398 //Or call it with declarations for sensor scales and data rates:
mitchpang 0:58395e885409 399 //uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
mitchpang 0:58395e885409 400 // dof.A_SCALE_2G, dof.M_SCALE_2GS);
mitchpang 0:58395e885409 401
mitchpang 0:58395e885409 402 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
mitchpang 0:58395e885409 403 imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
mitchpang 0:58395e885409 404
mitchpang 0:58395e885409 405 // begin() returns a 16-bit value which includes both the gyro
mitchpang 0:58395e885409 406 // and accelerometers WHO_AM_I response. You can check this to
mitchpang 0:58395e885409 407 // make sure communication was successful.
mitchpang 0:58395e885409 408 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
mitchpang 0:58395e885409 409 pc.printf("%x\n",status);
mitchpang 0:58395e885409 410 pc.printf("Should be 0x49D4\n");
mitchpang 0:58395e885409 411 pc.printf("\n");
mitchpang 0:58395e885409 412
mitchpang 0:58395e885409 413 // Set up PID Loops
mitchpang 0:58395e885409 414 linearController.setInputLimits(-1.0, 1.0);
mitchpang 0:58395e885409 415 angularController.setInputLimits(-1.0, 1.0);
mitchpang 0:58395e885409 416 linearController.setOutputLimits(0.0, 1.0);
mitchpang 0:58395e885409 417 angularController.setOutputLimits(0.0, 1.0);
mitchpang 0:58395e885409 418 linearController.setSetPoint(0.0);
mitchpang 0:58395e885409 419 angularController.setSetPoint(0.0);
mitchpang 0:58395e885409 420 linearController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 421 angularController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 422
mitchpang 0:58395e885409 423 }
mitchpang 0:58395e885409 424
mitchpang 0:58395e885409 425
mitchpang 0:58395e885409 426 int main() {
admcrae 4:6f6a9602e92d 427 float linearOutput = 0;
admcrae 4:6f6a9602e92d 428 float angularOutput = 0;
mitchpang 0:58395e885409 429
mitchpang 0:58395e885409 430 pc.printf("Starting IMU filter test...\n");
mitchpang 0:58395e885409 431 setup();
mitchpang 0:58395e885409 432
mitchpang 0:58395e885409 433
mitchpang 0:58395e885409 434 //Initialize inertial sensors.
mitchpang 0:58395e885409 435 calibrateAccelerometer();
mitchpang 0:58395e885409 436 calibrateGyroscope();
mitchpang 0:58395e885409 437
mitchpang 0:58395e885409 438 Thread accelThread(sampleAccelerometer);
mitchpang 0:58395e885409 439 Thread gyroThread(sampleGyroscope);
mitchpang 0:58395e885409 440 Thread commThread(parseInput);
mitchpang 0:58395e885409 441 commTicker.attach(&checkCOMRecieved, 2.0); // the address of the function to be attached (checkCOMRecieved) and the interval (2 seconds)
mitchpang 0:58395e885409 442 /*
mitchpang 0:58395e885409 443 pcMutex.lock();
mitchpang 0:58395e885409 444 pc.printf("done attaching tickers\n\n");
mitchpang 0:58395e885409 445 pcMutex.unlock();
mitchpang 0:58395e885409 446 */
mitchpang 0:58395e885409 447 while (1) {
mitchpang 0:58395e885409 448 //pc.printf("in loop\n");
admcrae 4:6f6a9602e92d 449 if(usePID) {
admcrae 4:6f6a9602e92d 450 linearController.setSetPoint(setLinear);
admcrae 4:6f6a9602e92d 451 angularController.setSetPoint(setAngular);
admcrae 4:6f6a9602e92d 452
mitchpang 0:58395e885409 453 v_mag2 = v_x * v_x + v_y * v_y;
mitchpang 0:58395e885409 454 linearController.setProcessValue(v_mag2);
mitchpang 0:58395e885409 455 angularController.setProcessValue(w_z); // w_z = degrees per second
mitchpang 0:58395e885409 456 linearOutput = linearController.compute();
mitchpang 0:58395e885409 457 angularOutput = angularController.compute();
admcrae 4:6f6a9602e92d 458 } else {
admcrae 4:6f6a9602e92d 459 linearOutput = setLinear;
admcrae 4:6f6a9602e92d 460 angularOutput = setAngular;
admcrae 4:6f6a9602e92d 461 }
admcrae 4:6f6a9602e92d 462
admcrae 4:6f6a9602e92d 463 if (enabled) {
admcrae 4:6f6a9602e92d 464 float leftMotorCO = (linearOutput + angularOutput);
admcrae 4:6f6a9602e92d 465 float rightMotorCO = (linearOutput - angularOutput);
admcrae 4:6f6a9602e92d 466
mitchpang 0:58395e885409 467 leftMotor.speed(leftMotorCO);
mitchpang 0:58395e885409 468 rightMotor.speed(rightMotorCO);
admcrae 4:6f6a9602e92d 469
admcrae 4:6f6a9602e92d 470
admcrae 4:6f6a9602e92d 471 /*
admcrae 4:6f6a9602e92d 472 leftMotor.speed(setLinear);
admcrae 4:6f6a9602e92d 473 rightMotor.speed(setAngular);
admcrae 4:6f6a9602e92d 474 */
admcrae 4:6f6a9602e92d 475 } else {
admcrae 4:6f6a9602e92d 476 leftMotor.coast();
admcrae 4:6f6a9602e92d 477 rightMotor.coast();
admcrae 4:6f6a9602e92d 478 }
admcrae 4:6f6a9602e92d 479
mitchpang 0:58395e885409 480 Thread::wait(RATE * 1000);
mitchpang 0:58395e885409 481 /*
mitchpang 0:58395e885409 482 pcMutex.lock();
mitchpang 0:58395e885409 483 pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
mitchpang 0:58395e885409 484 toDegrees(imuFilter.getPitch()),
mitchpang 0:58395e885409 485 toDegrees(imuFilter.getYaw()));
mitchpang 0:58395e885409 486 pcMutex.unlock();
mitchpang 0:58395e885409 487 */
mitchpang 0:58395e885409 488 }
mitchpang 0:58395e885409 489
mitchpang 0:58395e885409 490 }