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@9:b537a858040e, 2015-05-01 (annotated)

Committer:
mitchpang
Date:
Fri May 01 00:42:23 2015 +0000
Revision:
9:b537a858040e
Parent:
8:1aeb13d290db 
Working, we think.

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 6:c0a2ac7a8c26 7
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 6:c0a2ac7a8c26 32 //Multiply the linear and angular speed to change from [-1,1] to [-Multiplier, Multiplier]
mitchpang 6:c0a2ac7a8c26 33 #define LINEAR_MULTIPLIER 5
mitchpang 6:c0a2ac7a8c26 34 #define ANGULAR_MULTIPLIER 90
mitchpang 0:58395e885409 35
mitchpang 9:b537a858040e 36 #define LINEAR_MAX_SPEED 3.0
mitchpang 9:b537a858040e 37 #define ANGULAR_MAX_SPEED 90.0
mitchpang 9:b537a858040e 38
mitchpang 0:58395e885409 39 Serial pc(USBTX, USBRX);
mitchpang 0:58395e885409 40 LSM9DS0 imu(p28, p27, LSM9DS0_G, LSM9DS0_XM);
mitchpang 0:58395e885409 41 Mutex pcMutex;
mitchpang 7:bde99b0b3a86 42 Mutex setPointMutex;
mitchpang 7:bde99b0b3a86 43 Mutex processValueMutex;
admcrae 4:6f6a9602e92d 44 MODSERIAL xbee(p13, p14, 64, 128);
mitchpang 8:1aeb13d290db 45 PID linearController(1.0, 0.0, 0.0, RATE);
mitchpang 8:1aeb13d290db 46 PID angularController(1.0, 0.0, 0.0, RATE);
mitchpang 0:58395e885409 47 DigitalOut led1(LED1);
mitchpang 0:58395e885409 48 DigitalOut led2(LED2);
mitchpang 0:58395e885409 49 DigitalOut led3(LED3);
mitchpang 0:58395e885409 50 DigitalOut led4(LED4);
admcrae 4:6f6a9602e92d 51 int usePID = 1;
admcrae 4:6f6a9602e92d 52 int commReceived = 0;
mitchpang 0:58395e885409 53
mitchpang 0:58395e885409 54 //Gloabal Variables for comm system below
mitchpang 0:58395e885409 55 Ticker commTicker;
mitchpang 0:58395e885409 56
mitchpang 0:58395e885409 57
mitchpang 0:58395e885409 58 //Global Variables for motor control below
mitchpang 0:58395e885409 59 Motor leftMotor(p21, p22, p23, 1); // pwm, fwd, rev
mitchpang 0:58395e885409 60 Motor rightMotor(p26, p25, p24, 1); // pwm, fwd, rev
mitchpang 0:58395e885409 61
mitchpang 0:58395e885409 62 //Offsets for the gyroscope.
mitchpang 0:58395e885409 63 //The readings we take when the gyroscope is stationary won't be 0, so we'll
mitchpang 0:58395e885409 64 //average a set of readings we do get when the gyroscope is stationary and
mitchpang 0:58395e885409 65 //take those away from subsequent readings to ensure the gyroscope is offset
mitchpang 0:58395e885409 66 //or "biased" to 0.
mitchpang 0:58395e885409 67 double w_xBias;
mitchpang 0:58395e885409 68 double w_yBias;
mitchpang 0:58395e885409 69 double w_zBias;
mitchpang 6:c0a2ac7a8c26 70
mitchpang 0:58395e885409 71 //Offsets for the accelerometer.
mitchpang 0:58395e885409 72 //Same as with the gyroscope.
mitchpang 0:58395e885409 73 double a_xBias;
mitchpang 0:58395e885409 74 double a_yBias;
mitchpang 0:58395e885409 75 double a_zBias;
mitchpang 6:c0a2ac7a8c26 76
mitchpang 0:58395e885409 77 //Accumulators used for oversampling and then averaging.
mitchpang 0:58395e885409 78 double a_xAccumulator = 0;
mitchpang 0:58395e885409 79 double a_yAccumulator = 0;
mitchpang 0:58395e885409 80 double a_zAccumulator = 0;
mitchpang 0:58395e885409 81 double w_xAccumulator = 0;
mitchpang 0:58395e885409 82 double w_yAccumulator = 0;
mitchpang 0:58395e885409 83 double w_zAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 84
mitchpang 0:58395e885409 85 //Accelerometer and gyroscope readings for x, y, z axes.
mitchpang 0:58395e885409 86 double a_x;
mitchpang 0:58395e885409 87 double a_y;
mitchpang 0:58395e885409 88 double a_z;
mitchpang 0:58395e885409 89 double w_x;
mitchpang 0:58395e885409 90 double w_y;
mitchpang 0:58395e885409 91 double w_z;
mitchpang 0:58395e885409 92
mitchpang 0:58395e885409 93 //Previous Acceleromerter and gyroscope readings for integration
mitchpang 0:58395e885409 94 double last_a_x;
mitchpang 0:58395e885409 95 double last_a_y;
mitchpang 0:58395e885409 96 double last_a_z;
mitchpang 6:c0a2ac7a8c26 97
mitchpang 0:58395e885409 98 //Buffer for accelerometer readings.
mitchpang 0:58395e885409 99 int readings[3];
mitchpang 0:58395e885409 100 //Number of accelerometer samples we're on.
mitchpang 0:58395e885409 101 int accelerometerSamples = 0;
mitchpang 0:58395e885409 102 //Number of gyroscope samples we're on.
mitchpang 0:58395e885409 103 int gyroscopeSamples = 0;
mitchpang 0:58395e885409 104
mitchpang 0:58395e885409 105 //current readings of linear velocity
mitchpang 0:58395e885409 106 double v_x = 0;
mitchpang 0:58395e885409 107 double v_y = 0;
mitchpang 0:58395e885409 108 double v_z = 0;
mitchpang 0:58395e885409 109 double v_mag2 = 0; // linear velocity squared. sqrt() is comupationally difficult
mitchpang 6:c0a2ac7a8c26 110
mitchpang 6:c0a2ac7a8c26 111
mitchpang 0:58395e885409 112 //values for pwm wave to motors
mitchpang 8:1aeb13d290db 113
mitchpang 0:58395e885409 114 float linearOutput = 0;
mitchpang 0:58395e885409 115 float angularOutput = 0;
mitchpang 8:1aeb13d290db 116
mitchpang 6:c0a2ac7a8c26 117 float leftMotorCO = 0;
mitchpang 6:c0a2ac7a8c26 118 float rightMotorCO = 0;
mitchpang 6:c0a2ac7a8c26 119
admcrae 4:6f6a9602e92d 120
admcrae 4:6f6a9602e92d 121 //setpoints from xbee
admcrae 4:6f6a9602e92d 122 float setLinear = 0;
admcrae 4:6f6a9602e92d 123 float setAngular = 0;
admcrae 4:6f6a9602e92d 124 int enabled = 0;
mitchpang 8:1aeb13d290db 125 int lin_reverse;
mitchpang 8:1aeb13d290db 126 int ang_reverse;
admcrae 4:6f6a9602e92d 127
mitchpang 0:58395e885409 128 /**
mitchpang 0:58395e885409 129 * Prototypes
mitchpang 0:58395e885409 130 */
mitchpang 0:58395e885409 131 //Calculate the null bias.
mitchpang 0:58395e885409 132 void calibrateAccelerometer(void);
mitchpang 0:58395e885409 133 //Take a set of samples and average them.
mitchpang 0:58395e885409 134 void sampleAccelerometer(void);
mitchpang 0:58395e885409 135 //Calculate the null bias.
mitchpang 0:58395e885409 136 void calibrateGyroscope(void);
mitchpang 0:58395e885409 137 //Take a set of samples and average them.
mitchpang 0:58395e885409 138 void sampleGyroscope(void);
mitchpang 0:58395e885409 139
mitchpang 6:c0a2ac7a8c26 140
mitchpang 6:c0a2ac7a8c26 141
mitchpang 6:c0a2ac7a8c26 142 void debug(void const *args)
mitchpang 6:c0a2ac7a8c26 143 {
mitchpang 6:c0a2ac7a8c26 144 while(1) {
mitchpang 6:c0a2ac7a8c26 145 //pc.printf("linearOutput: %f angularOutput: %f\n",linearOutput, angularOutput);
mitchpang 6:c0a2ac7a8c26 146 //pc.printf("leftMotorCO: %f rightMotorCO: %f\n", leftMotorCO, rightMotorCO);
mitchpang 6:c0a2ac7a8c26 147 pc.printf("setLinear: %f setAngular: %f\n", setLinear, setAngular);
mitchpang 8:1aeb13d290db 148 pc.printf("v_x: %f angularSpeed: %f\n", v_x, w_z);
mitchpang 8:1aeb13d290db 149 pc.printf("linearOutput: %f angularOutput: %f\n", linearOutput, angularOutput);
mitchpang 7:bde99b0b3a86 150 pc.printf("leftMotorCO: %f rightMotorCO: %f\n\n", leftMotorCO, rightMotorCO);
mitchpang 6:c0a2ac7a8c26 151 Thread::wait(2000);
mitchpang 6:c0a2ac7a8c26 152 }
mitchpang 6:c0a2ac7a8c26 153 }
mitchpang 6:c0a2ac7a8c26 154
mitchpang 6:c0a2ac7a8c26 155
mitchpang 6:c0a2ac7a8c26 156 void startPID()
mitchpang 6:c0a2ac7a8c26 157 {
mitchpang 0:58395e885409 158 linearController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 159 angularController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 160 usePID = 1;
admcrae 4:6f6a9602e92d 161 }
mitchpang 6:c0a2ac7a8c26 162
mitchpang 6:c0a2ac7a8c26 163 void stopPID()
mitchpang 6:c0a2ac7a8c26 164 {
mitchpang 0:58395e885409 165 linearController.setMode(MANUAL_MODE);
mitchpang 0:58395e885409 166 angularController.setMode(MANUAL_MODE);
mitchpang 0:58395e885409 167 usePID = 0;
admcrae 4:6f6a9602e92d 168 }
admcrae 4:6f6a9602e92d 169
admcrae 4:6f6a9602e92d 170
mitchpang 8:1aeb13d290db 171 void parseCommand(char buff[], int length) {
mitchpang 6:c0a2ac7a8c26 172 float *tempForward, *tempAngular;
mitchpang 6:c0a2ac7a8c26 173 volatile char forwardBuffer [4];
mitchpang 6:c0a2ac7a8c26 174 volatile char angularBuffer [4];
admcrae 4:6f6a9602e92d 175 int tempEnabled, tempPID;
mitchpang 6:c0a2ac7a8c26 176 char statusChar;
mitchpang 6:c0a2ac7a8c26 177
mitchpang 8:1aeb13d290db 178 float *tempLinearPGain, *tempLinearIGain, *tempLinearDGain;
mitchpang 8:1aeb13d290db 179 float *tempAngularPGain, *tempAngularIGain, *tempAngularDGain;
mitchpang 6:c0a2ac7a8c26 180
mitchpang 8:1aeb13d290db 181 // Main control parsing
mitchpang 8:1aeb13d290db 182 if (0xCC == buff[0] && 10 == length) {
mitchpang 6:c0a2ac7a8c26 183
mitchpang 8:1aeb13d290db 184 tempForward = (float*) (buff + 1);
mitchpang 8:1aeb13d290db 185 tempAngular = (float*) (buff + 5);
mitchpang 8:1aeb13d290db 186 statusChar = buff[9]; //contains data for enable and enablePID
mitchpang 8:1aeb13d290db 187 tempPID = (int) (statusChar & 0x0F); //extract the last 4 bits and type cast to an int
mitchpang 8:1aeb13d290db 188 tempEnabled = (int) (statusChar & 0xF0); //extract the first 4 bits and type cast to an int
mitchpang 8:1aeb13d290db 189
mitchpang 8:1aeb13d290db 190 //Change global variables
mitchpang 8:1aeb13d290db 191 setPointMutex.lock();
mitchpang 9:b537a858040e 192 setLinear = *tempForward;
mitchpang 9:b537a858040e 193 setAngular = *tempAngular;
mitchpang 9:b537a858040e 194
mitchpang 9:b537a858040e 195 /*
mitchpang 8:1aeb13d290db 196 if (setLinear < 0) {
mitchpang 8:1aeb13d290db 197 setLinear = -setLinear; // make pos
mitchpang 8:1aeb13d290db 198 lin_reverse = 1;
mitchpang 8:1aeb13d290db 199 } else lin_reverse = 0;
mitchpang 6:c0a2ac7a8c26 200
mitchpang 8:1aeb13d290db 201 if (setAngular < 0) {
mitchpang 8:1aeb13d290db 202 setAngular = -setAngular; //make pos
mitchpang 8:1aeb13d290db 203 ang_reverse = 1;
mitchpang 8:1aeb13d290db 204 } else ang_reverse = 0;
mitchpang 9:b537a858040e 205 */
mitchpang 8:1aeb13d290db 206 enabled = tempEnabled;
mitchpang 8:1aeb13d290db 207 if (tempPID) {
mitchpang 8:1aeb13d290db 208 startPID();
mitchpang 8:1aeb13d290db 209 } else {
mitchpang 8:1aeb13d290db 210 stopPID();
mitchpang 8:1aeb13d290db 211 }
mitchpang 8:1aeb13d290db 212 setPointMutex.unlock();
mitchpang 8:1aeb13d290db 213 // Parsing change PID values command
mitchpang 8:1aeb13d290db 214 } else if (0x55 == buff[0] && 25 == length) {
mitchpang 8:1aeb13d290db 215 tempLinearPGain = (float*) (buff + 1);
mitchpang 8:1aeb13d290db 216 tempLinearIGain = (float*) (buff + 5);
mitchpang 8:1aeb13d290db 217 tempLinearDGain = (float*) (buff + 9);
mitchpang 8:1aeb13d290db 218 tempAngularPGain = (float*) (buff + 13);
mitchpang 8:1aeb13d290db 219 tempAngularIGain = (float*) (buff + 17);
mitchpang 8:1aeb13d290db 220 tempAngularDGain = (float*) (buff + 21);
mitchpang 8:1aeb13d290db 221 linearController.setTunings(*tempLinearPGain, *tempLinearIGain, *tempLinearDGain);
mitchpang 8:1aeb13d290db 222 angularController.setTunings(*tempAngularPGain, *tempAngularIGain, *tempAngularDGain);
mitchpang 0:58395e885409 223 }
mitchpang 6:c0a2ac7a8c26 224
admcrae 4:6f6a9602e92d 225 }
mitchpang 6:c0a2ac7a8c26 226
mitchpang 8:1aeb13d290db 227 char waitForChar() {
mitchpang 8:1aeb13d290db 228 while (!xbee.readable()) {
mitchpang 8:1aeb13d290db 229 Thread::wait(10);
mitchpang 8:1aeb13d290db 230 }
mitchpang 8:1aeb13d290db 231 return xbee.getc();
mitchpang 8:1aeb13d290db 232 }
mitchpang 8:1aeb13d290db 233
mitchpang 6:c0a2ac7a8c26 234 void parseInput(void const *args)
mitchpang 6:c0a2ac7a8c26 235 {
admcrae 4:6f6a9602e92d 236 static int i = 0;
mitchpang 8:1aeb13d290db 237 static int length = 0;
mitchpang 8:1aeb13d290db 238 static char commBuffer [64];
mitchpang 8:1aeb13d290db 239
mitchpang 8:1aeb13d290db 240 while (xbee.readable()) {
mitchpang 8:1aeb13d290db 241 xbee.getc();
mitchpang 8:1aeb13d290db 242 }
mitchpang 6:c0a2ac7a8c26 243
admcrae 4:6f6a9602e92d 244 while (1) {
mitchpang 8:1aeb13d290db 245 if (0x55 != waitForChar()) {
mitchpang 8:1aeb13d290db 246 continue;
mitchpang 6:c0a2ac7a8c26 247 }
mitchpang 8:1aeb13d290db 248 if (0x55 != waitForChar()) {
mitchpang 8:1aeb13d290db 249 continue;
admcrae 4:6f6a9602e92d 250 }
mitchpang 8:1aeb13d290db 251 length = waitForChar();
mitchpang 6:c0a2ac7a8c26 252
mitchpang 8:1aeb13d290db 253 for (i = 0; i < length; ++i) {
mitchpang 8:1aeb13d290db 254 commBuffer[i] = waitForChar();
mitchpang 6:c0a2ac7a8c26 255 }
mitchpang 8:1aeb13d290db 256 if (waitForChar() != 0xAA) {
mitchpang 8:1aeb13d290db 257 continue;
mitchpang 8:1aeb13d290db 258 }
mitchpang 8:1aeb13d290db 259 commReceived = 1;
mitchpang 8:1aeb13d290db 260 parseCommand(commBuffer, length);
mitchpang 8:1aeb13d290db 261 //Thread::wait(100);
admcrae 4:6f6a9602e92d 262 }
admcrae 4:6f6a9602e92d 263 }
mitchpang 6:c0a2ac7a8c26 264
mitchpang 0:58395e885409 265
mitchpang 6:c0a2ac7a8c26 266 void checkCOMRecieved()
mitchpang 6:c0a2ac7a8c26 267 {
mitchpang 6:c0a2ac7a8c26 268 if (commReceived == 0) {
mitchpang 6:c0a2ac7a8c26 269 stopPID();
mitchpang 6:c0a2ac7a8c26 270 enabled = 0;
mitchpang 6:c0a2ac7a8c26 271 led1 = led2 = led3 = led4 = 1;
admcrae 4:6f6a9602e92d 272 } else {
admcrae 4:6f6a9602e92d 273 led1 = led2 = led3 = led4 = 0;
mitchpang 0:58395e885409 274 }
admcrae 4:6f6a9602e92d 275 commReceived = 0;
mitchpang 0:58395e885409 276 }
mitchpang 0:58395e885409 277
mitchpang 6:c0a2ac7a8c26 278 void sampleAccelerometer(void const *args)
mitchpang 6:c0a2ac7a8c26 279 {
mitchpang 6:c0a2ac7a8c26 280 while(1) {
mitchpang 6:c0a2ac7a8c26 281 while(usePID != 1) {
mitchpang 6:c0a2ac7a8c26 282 Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
mitchpang 6:c0a2ac7a8c26 283 }
mitchpang 6:c0a2ac7a8c26 284 //Have we taken enough samples?
mitchpang 6:c0a2ac7a8c26 285 if (accelerometerSamples == SAMPLES) {
mitchpang 6:c0a2ac7a8c26 286
mitchpang 6:c0a2ac7a8c26 287 //Average the samples, remove the bias, and calculate the acceleration
mitchpang 6:c0a2ac7a8c26 288 //in m/s/s.
mitchpang 7:bde99b0b3a86 289 a_x = -((a_xAccumulator / SAMPLES) - a_xBias) * g0;// * ACCELEROMETER_GAIN;
mitchpang 7:bde99b0b3a86 290 a_y = -((a_yAccumulator / SAMPLES) - a_yBias) * g0;// * ACCELEROMETER_GAIN;
mitchpang 7:bde99b0b3a86 291 a_z = -((a_zAccumulator / SAMPLES) - a_zBias) * g0;// * ACCELEROMETER_GAIN;
mitchpang 6:c0a2ac7a8c26 292
mitchpang 6:c0a2ac7a8c26 293 a_xAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 294 a_yAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 295 a_zAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 296 accelerometerSamples = 0;
mitchpang 6:c0a2ac7a8c26 297 //pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 298 //pc.printf("a_x: %f a_y: %f a_z: %f \n",a_x,a_y,a_z);
mitchpang 6:c0a2ac7a8c26 299 //pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 300
mitchpang 6:c0a2ac7a8c26 301 // integrate to get velocity. make sure to subtract gravity downwards!
mitchpang 6:c0a2ac7a8c26 302
mitchpang 7:bde99b0b3a86 303 v_x = v_x + (a_x + last_a_x)/2 * ACC_RATE * SAMPLES;
mitchpang 7:bde99b0b3a86 304 v_y = v_y + (a_y + last_a_y)/2 * ACC_RATE * SAMPLES;
mitchpang 7:bde99b0b3a86 305 v_z = v_z + (a_z + last_a_z)/2 * ACC_RATE * SAMPLES;
mitchpang 6:c0a2ac7a8c26 306
mitchpang 6:c0a2ac7a8c26 307 last_a_x = a_x;
mitchpang 6:c0a2ac7a8c26 308 last_a_y = a_y;
mitchpang 6:c0a2ac7a8c26 309 last_a_z = a_z;
mitchpang 6:c0a2ac7a8c26 310 //pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 311 //pc.printf("v_x: %f v_y: %f v_z: %f \n",v_x,v_y,v_z);
mitchpang 6:c0a2ac7a8c26 312 //pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 313
mitchpang 6:c0a2ac7a8c26 314 } else {
mitchpang 6:c0a2ac7a8c26 315 //Take another sample.
mitchpang 6:c0a2ac7a8c26 316 imu.readAccel();
mitchpang 6:c0a2ac7a8c26 317 /*
mitchpang 6:c0a2ac7a8c26 318 pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 319 pc.printf("A: ");
mitchpang 6:c0a2ac7a8c26 320 pc.printf("%d", imu.ax);
mitchpang 6:c0a2ac7a8c26 321 pc.printf(", ");
mitchpang 6:c0a2ac7a8c26 322 pc.printf("%d",imu.ay);
mitchpang 6:c0a2ac7a8c26 323 pc.printf(", ");
mitchpang 6:c0a2ac7a8c26 324 pc.printf("%d\n",imu.az);
mitchpang 6:c0a2ac7a8c26 325 pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 326 */
mitchpang 6:c0a2ac7a8c26 327 a_xAccumulator += imu.ax;
mitchpang 6:c0a2ac7a8c26 328 a_yAccumulator += imu.ay;
mitchpang 6:c0a2ac7a8c26 329 a_zAccumulator += imu.az;
mitchpang 6:c0a2ac7a8c26 330
mitchpang 6:c0a2ac7a8c26 331 accelerometerSamples++;
mitchpang 6:c0a2ac7a8c26 332 Thread::wait(ACC_RATE * 1000);
mitchpang 6:c0a2ac7a8c26 333 }
mitchpang 0:58395e885409 334
mitchpang 0:58395e885409 335
mitchpang 6:c0a2ac7a8c26 336 }
mitchpang 0:58395e885409 337 }
mitchpang 6:c0a2ac7a8c26 338
mitchpang 6:c0a2ac7a8c26 339 void calibrateAccelerometer(void)
mitchpang 6:c0a2ac7a8c26 340 {
mitchpang 0:58395e885409 341
mitchpang 0:58395e885409 342 led1 = 1;
mitchpang 6:c0a2ac7a8c26 343
mitchpang 0:58395e885409 344 a_xAccumulator = 0;
mitchpang 0:58395e885409 345 a_yAccumulator = 0;
mitchpang 0:58395e885409 346 a_zAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 347
mitchpang 6:c0a2ac7a8c26 348
mitchpang 0:58395e885409 349 //Take a number of readings and average them
mitchpang 0:58395e885409 350 //to calculate the zero g offset.
mitchpang 0:58395e885409 351 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 6:c0a2ac7a8c26 352
mitchpang 0:58395e885409 353 imu.readAccel();
mitchpang 6:c0a2ac7a8c26 354
mitchpang 0:58395e885409 355 a_xAccumulator += (double) imu.ax;
mitchpang 0:58395e885409 356 a_yAccumulator += (double) imu.ay;
mitchpang 0:58395e885409 357 a_zAccumulator += (double) imu.az;
mitchpang 0:58395e885409 358
mitchpang 0:58395e885409 359 wait(ACC_RATE);
mitchpang 6:c0a2ac7a8c26 360
mitchpang 0:58395e885409 361 }
mitchpang 6:c0a2ac7a8c26 362
mitchpang 0:58395e885409 363 a_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 364 a_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 365 a_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 6:c0a2ac7a8c26 366
mitchpang 0:58395e885409 367 //At 4mg/LSB, 250 LSBs is 1g.
mitchpang 0:58395e885409 368 a_xBias = a_xAccumulator;
mitchpang 0:58395e885409 369 a_yBias = a_yAccumulator;
mitchpang 0:58395e885409 370 //a_zBias = (a_zAccumulator - (1/0.00006103515625));
mitchpang 0:58395e885409 371 a_zBias = a_zAccumulator; // calibrate so that gravity is already out of the equation
mitchpang 6:c0a2ac7a8c26 372 pc.printf("A_Bias ax: %f ay: %f az: %f \n", a_xBias,a_yBias,a_zBias);
mitchpang 6:c0a2ac7a8c26 373 /*
mitchpang 0:58395e885409 374 pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 375 pc.printf("A_Bias ax: %f ay: %f az: %f \n", a_xBias,a_yBias,a_zBias);
mitchpang 0:58395e885409 376 pc.printf("%f", a_xBias);
mitchpang 0:58395e885409 377 pc.printf(", ");
mitchpang 0:58395e885409 378 pc.printf("%f",a_yBias);
mitchpang 0:58395e885409 379 pc.printf(", ");
mitchpang 0:58395e885409 380 pc.printf("%f\n",a_zBias);
mitchpang 0:58395e885409 381 pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 382 */
mitchpang 0:58395e885409 383 a_xAccumulator = 0;
mitchpang 0:58395e885409 384 a_yAccumulator = 0;
mitchpang 0:58395e885409 385 a_zAccumulator = 0;
mitchpang 0:58395e885409 386 pc.printf("done calibrating accel\n");
mitchpang 0:58395e885409 387 led1 = 0;
mitchpang 0:58395e885409 388 }
mitchpang 6:c0a2ac7a8c26 389
mitchpang 6:c0a2ac7a8c26 390
mitchpang 6:c0a2ac7a8c26 391 void calibrateGyroscope(void)
mitchpang 6:c0a2ac7a8c26 392 {
mitchpang 0:58395e885409 393 led2 = 1;
mitchpang 0:58395e885409 394 w_xAccumulator = 0;
mitchpang 0:58395e885409 395 w_yAccumulator = 0;
mitchpang 0:58395e885409 396 w_zAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 397
mitchpang 0:58395e885409 398 //Take a number of readings and average them
mitchpang 0:58395e885409 399 //to calculate the gyroscope bias offset.
mitchpang 0:58395e885409 400 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
mitchpang 0:58395e885409 401 imu.readGyro();
mitchpang 0:58395e885409 402 w_xAccumulator += (double) imu.gx;
mitchpang 0:58395e885409 403 w_yAccumulator += (double) imu.gy;
mitchpang 0:58395e885409 404 w_zAccumulator += (double) imu.gz;
mitchpang 0:58395e885409 405 Thread::wait(GYRO_RATE * 1000);
mitchpang 6:c0a2ac7a8c26 406
mitchpang 0:58395e885409 407 }
mitchpang 6:c0a2ac7a8c26 408
mitchpang 0:58395e885409 409 //Average the samples.
mitchpang 0:58395e885409 410 w_xAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 411 w_yAccumulator /= CALIBRATION_SAMPLES;
mitchpang 0:58395e885409 412 w_zAccumulator /= CALIBRATION_SAMPLES;
mitchpang 6:c0a2ac7a8c26 413
mitchpang 0:58395e885409 414 w_xBias = w_xAccumulator;
mitchpang 0:58395e885409 415 w_yBias = w_yAccumulator;
mitchpang 0:58395e885409 416 w_zBias = w_zAccumulator;
mitchpang 6:c0a2ac7a8c26 417
mitchpang 0:58395e885409 418 pcMutex.lock();
mitchpang 0:58395e885409 419 pc.printf("G_Bias: ");
mitchpang 0:58395e885409 420 pc.printf("%f", w_xBias);
mitchpang 0:58395e885409 421 pc.printf(", ");
mitchpang 0:58395e885409 422 pc.printf("%f",w_yBias);
mitchpang 0:58395e885409 423 pc.printf(", ");
mitchpang 0:58395e885409 424 pc.printf("%f\n",w_zBias);
mitchpang 0:58395e885409 425 pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 426
mitchpang 0:58395e885409 427 w_xAccumulator = 0;
mitchpang 0:58395e885409 428 w_yAccumulator = 0;
mitchpang 0:58395e885409 429 w_zAccumulator = 0;
mitchpang 0:58395e885409 430 pc.printf("done calibrating gyro\n");
mitchpang 0:58395e885409 431 led2 = 0;
mitchpang 0:58395e885409 432 }
mitchpang 6:c0a2ac7a8c26 433
mitchpang 6:c0a2ac7a8c26 434 void sampleGyroscope(void const *args)
mitchpang 6:c0a2ac7a8c26 435 {
mitchpang 6:c0a2ac7a8c26 436 while(1) {
mitchpang 6:c0a2ac7a8c26 437 while(usePID != 1) {
mitchpang 6:c0a2ac7a8c26 438 Thread::wait(1000); //wait 1 sec and check if we are using the PID loop again.
mitchpang 0:58395e885409 439 }
mitchpang 6:c0a2ac7a8c26 440 //Have we taken enough samples?
mitchpang 6:c0a2ac7a8c26 441 if (gyroscopeSamples == SAMPLES) {
mitchpang 6:c0a2ac7a8c26 442
mitchpang 6:c0a2ac7a8c26 443 //Average the samples, remove the bias, and calculate the angular
mitchpang 6:c0a2ac7a8c26 444 //velocity in deg/s.
mitchpang 7:bde99b0b3a86 445 w_x = ((w_xAccumulator / SAMPLES) - w_xBias); //* GYROSCOPE_GAIN;
mitchpang 7:bde99b0b3a86 446 w_y = ((w_yAccumulator / SAMPLES) - w_yBias); //* GYROSCOPE_GAIN;
mitchpang 7:bde99b0b3a86 447 w_z = ((w_zAccumulator / SAMPLES) - w_zBias); //* GYROSCOPE_GAIN;
mitchpang 6:c0a2ac7a8c26 448 //pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 449 //pc.printf("w_x: %f w_y: %f w_z: %f \n",w_x,w_y,w_z);
mitchpang 6:c0a2ac7a8c26 450 //pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 451 w_xAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 452 w_yAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 453 w_zAccumulator = 0;
mitchpang 6:c0a2ac7a8c26 454 gyroscopeSamples = 0;
mitchpang 6:c0a2ac7a8c26 455
mitchpang 6:c0a2ac7a8c26 456 } else {
mitchpang 6:c0a2ac7a8c26 457 imu.readGyro();
mitchpang 6:c0a2ac7a8c26 458 /*
mitchpang 6:c0a2ac7a8c26 459 pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 460 pc.printf("G: ");
mitchpang 6:c0a2ac7a8c26 461 pc.printf("%d", imu.gx);
mitchpang 6:c0a2ac7a8c26 462 pc.printf(", ");
mitchpang 6:c0a2ac7a8c26 463 pc.printf("%d",imu.gy);
mitchpang 6:c0a2ac7a8c26 464 pc.printf(", ");
mitchpang 6:c0a2ac7a8c26 465 pc.printf("%d\n",imu.gz);
mitchpang 6:c0a2ac7a8c26 466 pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 467 */
mitchpang 6:c0a2ac7a8c26 468 w_xAccumulator += imu.gx;
mitchpang 6:c0a2ac7a8c26 469 w_yAccumulator += imu.gy;
mitchpang 6:c0a2ac7a8c26 470 w_zAccumulator += imu.gz;
mitchpang 6:c0a2ac7a8c26 471
mitchpang 6:c0a2ac7a8c26 472 gyroscopeSamples++;
mitchpang 6:c0a2ac7a8c26 473 Thread::wait(GYRO_RATE * 1000);
mitchpang 6:c0a2ac7a8c26 474 }
mitchpang 6:c0a2ac7a8c26 475
mitchpang 0:58395e885409 476 Thread::wait(GYRO_RATE * 1000);
mitchpang 0:58395e885409 477 }
mitchpang 6:c0a2ac7a8c26 478 }
mitchpang 0:58395e885409 479
mitchpang 6:c0a2ac7a8c26 480
mitchpang 0:58395e885409 481
mitchpang 6:c0a2ac7a8c26 482
mitchpang 6:c0a2ac7a8c26 483 void setup()
mitchpang 0:58395e885409 484 {
mitchpang 0:58395e885409 485 pc.baud(9600); // Start serial at 115200 bps
mitchpang 0:58395e885409 486 // Use the begin() function to initialize the LSM9DS0 library.
mitchpang 0:58395e885409 487 // You can either call it with no parameters (the easy way):
mitchpang 0:58395e885409 488 //uint16_t status = dof.begin();
mitchpang 6:c0a2ac7a8c26 489
mitchpang 0:58395e885409 490 //Or call it with declarations for sensor scales and data rates:
mitchpang 0:58395e885409 491 //uint16_t status = imu.begin(dof.G_SCALE_2000DPS,
mitchpang 0:58395e885409 492 // dof.A_SCALE_2G, dof.M_SCALE_2GS);
mitchpang 6:c0a2ac7a8c26 493
mitchpang 0:58395e885409 494 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G, imu.M_SCALE_2GS,
mitchpang 0:58395e885409 495 imu.G_ODR_760_BW_100, imu.A_ODR_400, imu.M_ODR_50);
mitchpang 6:c0a2ac7a8c26 496
mitchpang 0:58395e885409 497 // begin() returns a 16-bit value which includes both the gyro
mitchpang 0:58395e885409 498 // and accelerometers WHO_AM_I response. You can check this to
mitchpang 0:58395e885409 499 // make sure communication was successful.
mitchpang 0:58395e885409 500 pc.printf("LSM9DS0 WHO_AM_I's returned: 0x");
mitchpang 0:58395e885409 501 pc.printf("%x\n",status);
mitchpang 0:58395e885409 502 pc.printf("Should be 0x49D4\n");
mitchpang 0:58395e885409 503 pc.printf("\n");
mitchpang 6:c0a2ac7a8c26 504
mitchpang 0:58395e885409 505 // Set up PID Loops
mitchpang 9:b537a858040e 506 linearController.setInputLimits(-1.0, 1.0);
mitchpang 9:b537a858040e 507 angularController.setInputLimits(-1.0, 1.0);
mitchpang 9:b537a858040e 508 linearController.setOutputLimits(-1.0, 1.0);
mitchpang 9:b537a858040e 509 angularController.setOutputLimits(-1.0, 1.0);
mitchpang 0:58395e885409 510 linearController.setSetPoint(0.0);
mitchpang 0:58395e885409 511 angularController.setSetPoint(0.0);
mitchpang 0:58395e885409 512 linearController.setMode(AUTO_MODE);
mitchpang 0:58395e885409 513 angularController.setMode(AUTO_MODE);
mitchpang 6:c0a2ac7a8c26 514
mitchpang 0:58395e885409 515 }
mitchpang 0:58395e885409 516
mitchpang 0:58395e885409 517
mitchpang 6:c0a2ac7a8c26 518 int main()
mitchpang 6:c0a2ac7a8c26 519 {
mitchpang 8:1aeb13d290db 520 //float linearOutput = 0;
mitchpang 8:1aeb13d290db 521 //float angularOutput = 0;
mitchpang 8:1aeb13d290db 522
mitchpang 6:c0a2ac7a8c26 523 /*
mitchpang 6:c0a2ac7a8c26 524 float leftMotorCO = 0;
mitchpang 6:c0a2ac7a8c26 525 float rightMotorCO = 0;
mitchpang 6:c0a2ac7a8c26 526 */
mitchpang 0:58395e885409 527 pc.printf("Starting IMU filter test...\n");
mitchpang 0:58395e885409 528 setup();
mitchpang 6:c0a2ac7a8c26 529
mitchpang 6:c0a2ac7a8c26 530
mitchpang 0:58395e885409 531 //Initialize inertial sensors.
mitchpang 0:58395e885409 532 calibrateAccelerometer();
mitchpang 0:58395e885409 533 calibrateGyroscope();
mitchpang 0:58395e885409 534
mitchpang 0:58395e885409 535 Thread accelThread(sampleAccelerometer);
mitchpang 0:58395e885409 536 Thread gyroThread(sampleGyroscope);
mitchpang 0:58395e885409 537 Thread commThread(parseInput);
mitchpang 6:c0a2ac7a8c26 538 Thread debugThread(debug);
mitchpang 0:58395e885409 539 commTicker.attach(&checkCOMRecieved, 2.0); // the address of the function to be attached (checkCOMRecieved) and the interval (2 seconds)
mitchpang 0:58395e885409 540 /*
mitchpang 6:c0a2ac7a8c26 541 //pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 542 pc.printf("done attaching tickers\n\n");
mitchpang 6:c0a2ac7a8c26 543 //pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 544 */
mitchpang 0:58395e885409 545 while (1) {
mitchpang 6:c0a2ac7a8c26 546 //pc.printf("in loop\n");
admcrae 4:6f6a9602e92d 547 if(usePID) {
mitchpang 7:bde99b0b3a86 548 setPointMutex.lock();
mitchpang 7:bde99b0b3a86 549 linearController.setSetPoint(setLinear);
admcrae 4:6f6a9602e92d 550 angularController.setSetPoint(setAngular);
mitchpang 7:bde99b0b3a86 551 setPointMutex.unlock();
mitchpang 7:bde99b0b3a86 552 if (abs(w_z) < 2.0f) w_z = 0; // if less than 2 deg/sec, assume it is noise and make it eqal to 0.
mitchpang 7:bde99b0b3a86 553 //v_mag2 = v_x * v_x + v_y * v_y;
mitchpang 7:bde99b0b3a86 554 processValueMutex.lock();
mitchpang 9:b537a858040e 555 linearController.setProcessValue(v_x/LINEAR_MAX_SPEED);
mitchpang 9:b537a858040e 556 angularController.setProcessValue(w_z/ANGULAR_MAX_SPEED); // w_z = degrees per second
mitchpang 7:bde99b0b3a86 557 processValueMutex.unlock();
mitchpang 8:1aeb13d290db 558 //pc.printf("linearOutput: %f",linearOutput);
mitchpang 9:b537a858040e 559 linearOutput = linearController.compute();
mitchpang 9:b537a858040e 560 angularOutput = angularController.compute();
mitchpang 9:b537a858040e 561
mitchpang 9:b537a858040e 562 /*
mitchpang 8:1aeb13d290db 563 if (lin_reverse == 0) {
mitchpang 8:1aeb13d290db 564 linearOutput = linearController.compute();
mitchpang 8:1aeb13d290db 565 } else {
mitchpang 8:1aeb13d290db 566 linearOutput = -linearController.compute();
mitchpang 8:1aeb13d290db 567 }
mitchpang 8:1aeb13d290db 568 if (ang_reverse == 0) {
mitchpang 8:1aeb13d290db 569 angularOutput = angularController.compute();
mitchpang 8:1aeb13d290db 570 } else {
mitchpang 8:1aeb13d290db 571 angularOutput = -angularController.compute();
mitchpang 8:1aeb13d290db 572 }
mitchpang 9:b537a858040e 573 */
mitchpang 9:b537a858040e 574
mitchpang 9:b537a858040e 575 } else {// PID Disabled
mitchpang 9:b537a858040e 576 linearOutput = setLinear;
mitchpang 9:b537a858040e 577 angularOutput = setAngular;
mitchpang 9:b537a858040e 578 }
mitchpang 9:b537a858040e 579
mitchpang 9:b537a858040e 580 if (enabled) {
mitchpang 9:b537a858040e 581
mitchpang 6:c0a2ac7a8c26 582 leftMotorCO = (linearOutput - angularOutput);
mitchpang 6:c0a2ac7a8c26 583 rightMotorCO = (linearOutput + angularOutput);
mitchpang 6:c0a2ac7a8c26 584
mitchpang 0:58395e885409 585 leftMotor.speed(leftMotorCO);
mitchpang 0:58395e885409 586 rightMotor.speed(rightMotorCO);
mitchpang 6:c0a2ac7a8c26 587
mitchpang 6:c0a2ac7a8c26 588
admcrae 4:6f6a9602e92d 589 /*
admcrae 4:6f6a9602e92d 590 leftMotor.speed(setLinear);
admcrae 4:6f6a9602e92d 591 rightMotor.speed(setAngular);
admcrae 4:6f6a9602e92d 592 */
admcrae 4:6f6a9602e92d 593 } else {
admcrae 4:6f6a9602e92d 594 leftMotor.coast();
admcrae 4:6f6a9602e92d 595 rightMotor.coast();
admcrae 4:6f6a9602e92d 596 }
mitchpang 6:c0a2ac7a8c26 597
mitchpang 0:58395e885409 598 Thread::wait(RATE * 1000);
mitchpang 6:c0a2ac7a8c26 599 /*
mitchpang 6:c0a2ac7a8c26 600 //pcMutex.lock();
mitchpang 6:c0a2ac7a8c26 601 pc.printf("%f,%f,%f\n",toDegrees(imuFilter.getRoll()),
mitchpang 6:c0a2ac7a8c26 602 toDegrees(imuFilter.getPitch()),
mitchpang 6:c0a2ac7a8c26 603 toDegrees(imuFilter.getYaw()));
mitchpang 6:c0a2ac7a8c26 604 //pcMutex.unlock();
mitchpang 6:c0a2ac7a8c26 605 */
mitchpang 0:58395e885409 606 }
mitchpang 0:58395e885409 607
mitchpang 0:58395e885409 608 }