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@8:1aeb13d290db, 2015-04-30 (annotated)

Committer:
mitchpang
Date:
Thu Apr 30 23:58:54 2015 +0000
Revision:
8:1aeb13d290db
Parent:
7:bde99b0b3a86
Child:
9:b537a858040e
Saved before editing PID loop stuff.

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