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@0:58395e885409, 2015-04-28 (annotated)

Committer:
mitchpang
Date:
Tue Apr 28 20:02:30 2015 +0000
Revision:
0:58395e885409
Child:
4:6f6a9602e92d
first commit

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