TurtleBot / Mbed 2 deprecated TurtleBot_V1

servo_tread + imu

Dependencies:   Servo mbed-rtos mbed

Fork of Turtlecase by TurtleBot

main.cpp@1:5609c1795245, 2018-02-20 (annotated)

Committer:
worasuchad
Date:
Tue Feb 20 11:06:35 2018 +0000
Revision:
1:5609c1795245
Parent:
0:812929a5d5ad 
servo + imu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
worasuchad 1:5609c1795245 1 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 2 // project: TurtleBot Project //
worasuchad 1:5609c1795245 3 // code v.: 1.0 //
worasuchad 1:5609c1795245 4 // board : NUCLEO-F303KB //
worasuchad 1:5609c1795245 5 // date : 20/2/2018 //
worasuchad 1:5609c1795245 6 // code by: Coding on Earth by Humans //
worasuchad 1:5609c1795245 7 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 8
worasuchad 1:5609c1795245 9 ///////////////////////// init ////////////////////////////////
worasuchad 1:5609c1795245 10 //////////////////////////////////////////////////////////////////
Khanchana 0:812929a5d5ad 11 #include "mbed.h"
Khanchana 0:812929a5d5ad 12 #include "rtos.h"
Khanchana 0:812929a5d5ad 13
Khanchana 0:812929a5d5ad 14 Serial pc(USBTX, USBRX);
worasuchad 1:5609c1795245 15
worasuchad 1:5609c1795245 16 Thread thread1; //control servo left
worasuchad 1:5609c1795245 17 Thread thread2; //control servo right
worasuchad 1:5609c1795245 18 Thread thread3; //read data from IMU
worasuchad 1:5609c1795245 19
worasuchad 1:5609c1795245 20 ///////////////////////// IMU ////////////////////////////////
worasuchad 1:5609c1795245 21 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 22 #include "MPU9250.h"
worasuchad 1:5609c1795245 23
worasuchad 1:5609c1795245 24 float sum = 0;
worasuchad 1:5609c1795245 25 uint32_t sumCount = 0;
worasuchad 1:5609c1795245 26 char buffer[14];
worasuchad 1:5609c1795245 27 float origin = 0;
worasuchad 1:5609c1795245 28
worasuchad 1:5609c1795245 29 MPU9250 mpu9250;
worasuchad 1:5609c1795245 30 Timer t;
worasuchad 1:5609c1795245 31
worasuchad 1:5609c1795245 32
worasuchad 1:5609c1795245 33 ///////////////////////// Servo ////////////////////////////////
worasuchad 1:5609c1795245 34 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 35 #include "Servo.h"
worasuchad 1:5609c1795245 36 Servo Servo1(D10);
Khanchana 0:812929a5d5ad 37 Servo Servo2(D6);
worasuchad 1:5609c1795245 38 Servo Servo3(D8);
worasuchad 1:5609c1795245 39 Servo Servo4(D9);
worasuchad 1:5609c1795245 40 /*
Khanchana 0:812929a5d5ad 41 int pos_up_start;
Khanchana 0:812929a5d5ad 42 int pos_up_end;
Khanchana 0:812929a5d5ad 43 int pos_down_start;
worasuchad 1:5609c1795245 44 int pos_down_end;*/
Khanchana 0:812929a5d5ad 45
worasuchad 1:5609c1795245 46 int pos_down_start = 1400;
worasuchad 1:5609c1795245 47 int pos_down_end = 1600;
worasuchad 1:5609c1795245 48 int pos_up_start = 1000;
worasuchad 1:5609c1795245 49 int pos_up_end = 1600;
Khanchana 0:812929a5d5ad 50
worasuchad 1:5609c1795245 51 ///////////////////////// prototype func ///////////////////////
worasuchad 1:5609c1795245 52 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 53 void myservoLeft();
worasuchad 1:5609c1795245 54 void myservoRight();
worasuchad 1:5609c1795245 55 void IMU();
Khanchana 0:812929a5d5ad 56
worasuchad 1:5609c1795245 57 ///////////////////////// main ////////////////////////////
worasuchad 1:5609c1795245 58 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 59 int main()
worasuchad 1:5609c1795245 60 {
worasuchad 1:5609c1795245 61 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 62 thread2.start(myservoRight);
worasuchad 1:5609c1795245 63 IMU();
worasuchad 1:5609c1795245 64 /* while(1)
worasuchad 1:5609c1795245 65 {
Khanchana 0:812929a5d5ad 66 printf("Hello World! Turtlebot is READY\n");
Khanchana 0:812929a5d5ad 67 printf("case 1-5\n");
worasuchad 1:5609c1795245 68 switch(pc.getc())
worasuchad 1:5609c1795245 69 {
Khanchana 0:812929a5d5ad 70 case '1':
Khanchana 0:812929a5d5ad 71 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 72 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 73 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 74 pos_up_end = 1700;
Khanchana 0:812929a5d5ad 75 break;
Khanchana 0:812929a5d5ad 76 case '2':
Khanchana 0:812929a5d5ad 77 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 78 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 79 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 80 pos_up_end = 1600;
Khanchana 0:812929a5d5ad 81 break;
Khanchana 0:812929a5d5ad 82 case '3':
Khanchana 0:812929a5d5ad 83 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 84 pos_down_end = 1650;
Khanchana 0:812929a5d5ad 85 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 86 pos_up_end = 1500;
Khanchana 0:812929a5d5ad 87 break;
Khanchana 0:812929a5d5ad 88 case '4':
Khanchana 0:812929a5d5ad 89 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 90 pos_down_end = 1700;
Khanchana 0:812929a5d5ad 91 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 92 pos_up_end = 1650;
Khanchana 0:812929a5d5ad 93 break;
Khanchana 0:812929a5d5ad 94 case '5':
Khanchana 0:812929a5d5ad 95 pos_down_start = 1400;
Khanchana 0:812929a5d5ad 96 pos_down_end = 1600;
Khanchana 0:812929a5d5ad 97 pos_up_start = 1000;
Khanchana 0:812929a5d5ad 98 pos_up_end = 1550;
Khanchana 0:812929a5d5ad 99 break;
Khanchana 0:812929a5d5ad 100 }
Khanchana 0:812929a5d5ad 101 printf("position down motor start = %d\n", pos_down_start);
Khanchana 0:812929a5d5ad 102 printf("position down motor end = %d\n", pos_down_end);
Khanchana 0:812929a5d5ad 103 printf("position up motor start = %d\n", pos_up_start);
Khanchana 0:812929a5d5ad 104 printf("position up motor end = %d\n", pos_up_end);
worasuchad 1:5609c1795245 105 thread1.start(myservoLeft);
worasuchad 1:5609c1795245 106 thread2.start(myservoRight);
worasuchad 1:5609c1795245 107 thread3.start(IMU);
worasuchad 1:5609c1795245 108 } */
worasuchad 1:5609c1795245 109 }
worasuchad 1:5609c1795245 110
worasuchad 1:5609c1795245 111
worasuchad 1:5609c1795245 112 ///////////////////////// myservoLeft /////////////////////////
worasuchad 1:5609c1795245 113 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 114 void myservoLeft()
worasuchad 1:5609c1795245 115 {
worasuchad 1:5609c1795245 116 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 117 {
worasuchad 1:5609c1795245 118 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 119 Servo2.Disable();
worasuchad 1:5609c1795245 120
worasuchad 1:5609c1795245 121 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 122 {
worasuchad 1:5609c1795245 123 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 124 wait(0.01);
worasuchad 1:5609c1795245 125 }
worasuchad 1:5609c1795245 126
worasuchad 1:5609c1795245 127 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 128 Servo1.Disable();
worasuchad 1:5609c1795245 129
worasuchad 1:5609c1795245 130 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 131 {
worasuchad 1:5609c1795245 132 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 133 wait(0.01);
worasuchad 1:5609c1795245 134 }
worasuchad 1:5609c1795245 135
worasuchad 1:5609c1795245 136 Servo1.Enable(1000,20000);
worasuchad 1:5609c1795245 137 Servo2.Disable();
worasuchad 1:5609c1795245 138
worasuchad 1:5609c1795245 139 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 140 {
worasuchad 1:5609c1795245 141 Servo1.SetPosition(pos);
worasuchad 1:5609c1795245 142 wait(0.01);
worasuchad 1:5609c1795245 143 }
worasuchad 1:5609c1795245 144
worasuchad 1:5609c1795245 145 Servo2.Enable(1000,20000);
worasuchad 1:5609c1795245 146 Servo1.Disable();
worasuchad 1:5609c1795245 147
worasuchad 1:5609c1795245 148 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 149 {
worasuchad 1:5609c1795245 150 Servo2.SetPosition(pos);
worasuchad 1:5609c1795245 151 wait(0.01);
worasuchad 1:5609c1795245 152 }
worasuchad 1:5609c1795245 153 }
worasuchad 1:5609c1795245 154 }
worasuchad 1:5609c1795245 155
worasuchad 1:5609c1795245 156 ///////////////////////// myservoRight ///////////////////////
worasuchad 1:5609c1795245 157 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 158 void myservoRight()
worasuchad 1:5609c1795245 159 {
worasuchad 1:5609c1795245 160 for(int n = 0; n <= 5; n += 1)
worasuchad 1:5609c1795245 161 {
worasuchad 1:5609c1795245 162 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 163 Servo4.Disable();
worasuchad 1:5609c1795245 164
worasuchad 1:5609c1795245 165 for (int pos = pos_down_start; pos <= pos_down_end; pos += 5)
worasuchad 1:5609c1795245 166 {
worasuchad 1:5609c1795245 167 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 168 wait(0.01);
worasuchad 1:5609c1795245 169 }
worasuchad 1:5609c1795245 170
worasuchad 1:5609c1795245 171 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 172 Servo3.Disable();
worasuchad 1:5609c1795245 173
worasuchad 1:5609c1795245 174 for (int pos = pos_up_start; pos <= pos_up_end; pos += 5)
worasuchad 1:5609c1795245 175 {
worasuchad 1:5609c1795245 176 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 177 wait(0.01);
worasuchad 1:5609c1795245 178 }
worasuchad 1:5609c1795245 179
worasuchad 1:5609c1795245 180 Servo3.Enable(1000,20000);
worasuchad 1:5609c1795245 181 Servo4.Disable();
worasuchad 1:5609c1795245 182
worasuchad 1:5609c1795245 183 for (int pos = pos_down_end; pos >= pos_down_start; pos -= 5)
worasuchad 1:5609c1795245 184 {
worasuchad 1:5609c1795245 185 Servo3.SetPosition(pos);
worasuchad 1:5609c1795245 186 wait(0.01);
worasuchad 1:5609c1795245 187 }
worasuchad 1:5609c1795245 188
worasuchad 1:5609c1795245 189 Servo4.Enable(1000,20000);
worasuchad 1:5609c1795245 190 Servo3.Disable();
worasuchad 1:5609c1795245 191
worasuchad 1:5609c1795245 192 for (int pos = pos_up_end; pos >= pos_up_start; pos -= 5)
worasuchad 1:5609c1795245 193 {
worasuchad 1:5609c1795245 194 Servo4.SetPosition(pos);
worasuchad 1:5609c1795245 195 wait(0.01);
worasuchad 1:5609c1795245 196 }
Khanchana 0:812929a5d5ad 197 }
worasuchad 1:5609c1795245 198 }
worasuchad 1:5609c1795245 199
worasuchad 1:5609c1795245 200 ///////////////////////// IMU ///////////////////////
worasuchad 1:5609c1795245 201 //////////////////////////////////////////////////////////////////
worasuchad 1:5609c1795245 202 void IMU()
worasuchad 1:5609c1795245 203 {
worasuchad 1:5609c1795245 204 //Set up I2C
worasuchad 1:5609c1795245 205 i2c.frequency(400000); // use fast (400 kHz) I2C
worasuchad 1:5609c1795245 206
worasuchad 1:5609c1795245 207 //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
worasuchad 1:5609c1795245 208 t.start();
worasuchad 1:5609c1795245 209
worasuchad 1:5609c1795245 210 // Read the WHO_AM_I register, this is a good test of communication
worasuchad 1:5609c1795245 211 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
worasuchad 1:5609c1795245 212 //pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
worasuchad 1:5609c1795245 213
worasuchad 1:5609c1795245 214 if (whoami == 0x73 ) // WHO_AM_I should always be 0x68
worasuchad 1:5609c1795245 215 {
worasuchad 1:5609c1795245 216 //pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
worasuchad 1:5609c1795245 217 //pc.printf("MPU9250 is online...\n\r");
worasuchad 1:5609c1795245 218 sprintf(buffer, "0x%x", whoami);
worasuchad 1:5609c1795245 219 wait(1);
worasuchad 1:5609c1795245 220
worasuchad 1:5609c1795245 221 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
worasuchad 1:5609c1795245 222 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
worasuchad 1:5609c1795245 223 //pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
worasuchad 1:5609c1795245 224 //pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
worasuchad 1:5609c1795245 225 //pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
worasuchad 1:5609c1795245 226 //pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
worasuchad 1:5609c1795245 227 //pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
worasuchad 1:5609c1795245 228 //pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
worasuchad 1:5609c1795245 229 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
worasuchad 1:5609c1795245 230 //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
worasuchad 1:5609c1795245 231 //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
worasuchad 1:5609c1795245 232 //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
worasuchad 1:5609c1795245 233 //pc.printf("x accel bias = %f\n\r", accelBias[0]);
worasuchad 1:5609c1795245 234 //pc.printf("y accel bias = %f\n\r", accelBias[1]);
worasuchad 1:5609c1795245 235 //pc.printf("z accel bias = %f\n\r", accelBias[2]);
worasuchad 1:5609c1795245 236 wait(2);
worasuchad 1:5609c1795245 237 mpu9250.initMPU9250();
worasuchad 1:5609c1795245 238 //pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
worasuchad 1:5609c1795245 239 mpu9250.initAK8963(magCalibration);
worasuchad 1:5609c1795245 240 //pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
worasuchad 1:5609c1795245 241 //pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
worasuchad 1:5609c1795245 242 //pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
worasuchad 1:5609c1795245 243
worasuchad 1:5609c1795245 244 if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
worasuchad 1:5609c1795245 245 if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
worasuchad 1:5609c1795245 246 if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
worasuchad 1:5609c1795245 247 if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
worasuchad 1:5609c1795245 248 wait(1);
worasuchad 1:5609c1795245 249 }
worasuchad 1:5609c1795245 250 else
worasuchad 1:5609c1795245 251 {
worasuchad 1:5609c1795245 252 pc.printf("Could not connect to MPU9250: \n\r");
worasuchad 1:5609c1795245 253 pc.printf("%#x \n", whoami);
worasuchad 1:5609c1795245 254
worasuchad 1:5609c1795245 255 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
worasuchad 1:5609c1795245 256
worasuchad 1:5609c1795245 257 while(1) ; // Loop forever if communication doesn't happen
worasuchad 1:5609c1795245 258 }
worasuchad 1:5609c1795245 259
worasuchad 1:5609c1795245 260 mpu9250.getAres(); // Get accelerometer sensitivity
worasuchad 1:5609c1795245 261 mpu9250.getGres(); // Get gyro sensitivity
worasuchad 1:5609c1795245 262 mpu9250.getMres(); // Get magnetometer sensitivity
worasuchad 1:5609c1795245 263 //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
worasuchad 1:5609c1795245 264 //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
worasuchad 1:5609c1795245 265 //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
worasuchad 1:5609c1795245 266 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
worasuchad 1:5609c1795245 267 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
worasuchad 1:5609c1795245 268 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
worasuchad 1:5609c1795245 269
worasuchad 1:5609c1795245 270 while(1)
worasuchad 1:5609c1795245 271 {
worasuchad 1:5609c1795245 272 // If intPin goes high, all data registers have new data
worasuchad 1:5609c1795245 273 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
worasuchad 1:5609c1795245 274
worasuchad 1:5609c1795245 275 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 276 // Now we'll calculate the accleration value into actual g's
worasuchad 1:5609c1795245 277 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
worasuchad 1:5609c1795245 278 ay = (float)accelCount[1]*aRes - accelBias[1];
worasuchad 1:5609c1795245 279 az = (float)accelCount[2]*aRes - accelBias[2];
worasuchad 1:5609c1795245 280
worasuchad 1:5609c1795245 281 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 282 // Calculate the gyro value into actual degrees per second
worasuchad 1:5609c1795245 283 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
worasuchad 1:5609c1795245 284 gy = (float)gyroCount[1]*gRes - gyroBias[1];
worasuchad 1:5609c1795245 285 gz = (float)gyroCount[2]*gRes - gyroBias[2];
worasuchad 1:5609c1795245 286
worasuchad 1:5609c1795245 287 mpu9250.readMagData(magCount); // Read the x/y/z adc values
worasuchad 1:5609c1795245 288 // Calculate the magnetometer values in milliGauss
worasuchad 1:5609c1795245 289 // Include factory calibration per data sheet and user environmental corrections
worasuchad 1:5609c1795245 290 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
worasuchad 1:5609c1795245 291 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
worasuchad 1:5609c1795245 292 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
worasuchad 1:5609c1795245 293 }
worasuchad 1:5609c1795245 294
worasuchad 1:5609c1795245 295 Now = t.read_us();
worasuchad 1:5609c1795245 296 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
worasuchad 1:5609c1795245 297 lastUpdate = Now;
worasuchad 1:5609c1795245 298
worasuchad 1:5609c1795245 299 sum += deltat;
worasuchad 1:5609c1795245 300 sumCount++;
worasuchad 1:5609c1795245 301
worasuchad 1:5609c1795245 302 //if(lastUpdate - firstUpdate > 10000000.0f)
worasuchad 1:5609c1795245 303 //{
worasuchad 1:5609c1795245 304 //beta = 0.04; // decrease filter gain after stabilized
worasuchad 1:5609c1795245 305 //zeta = 0.015; // increasey bias drift gain after stabilized
worasuchad 1:5609c1795245 306 //}
worasuchad 1:5609c1795245 307
worasuchad 1:5609c1795245 308 //Pass gyro rate as rad/s
worasuchad 1:5609c1795245 309 //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
worasuchad 1:5609c1795245 310 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
worasuchad 1:5609c1795245 311
worasuchad 1:5609c1795245 312 //Serial print and/or display at 0.5 s rate independent of data rates
worasuchad 1:5609c1795245 313 delt_t = t.read_ms() - count;
worasuchad 1:5609c1795245 314
worasuchad 1:5609c1795245 315 if (delt_t > 50)
worasuchad 1:5609c1795245 316 { // update LCD once per half-second independent of read rate
worasuchad 1:5609c1795245 317
worasuchad 1:5609c1795245 318 //pc.printf("ax = %f", 1000*ax);
worasuchad 1:5609c1795245 319 //pc.printf(" ay = %f", 1000*ay);
worasuchad 1:5609c1795245 320 //pc.printf(" az = %f mg\n\r", 1000*az);
worasuchad 1:5609c1795245 321
worasuchad 1:5609c1795245 322 //pc.printf("gx = %f", gx);
worasuchad 1:5609c1795245 323 //pc.printf(" gy = %f", gy);
worasuchad 1:5609c1795245 324 //pc.printf(" gz = %f deg/s\n\r", gz);
worasuchad 1:5609c1795245 325
worasuchad 1:5609c1795245 326 //pc.printf("gx = %f", mx);
worasuchad 1:5609c1795245 327 //pc.printf(" gy = %f", my);
worasuchad 1:5609c1795245 328 //pc.printf(" gz = %f mG\n\r", mz);
worasuchad 1:5609c1795245 329
worasuchad 1:5609c1795245 330 //tempCount = mpu9250.readTempData(); // Read the adc values
worasuchad 1:5609c1795245 331 //temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
worasuchad 1:5609c1795245 332 //pc.printf(" temperature = %f C\n\r", temperature);
worasuchad 1:5609c1795245 333
worasuchad 1:5609c1795245 334 //pc.printf("q0 = %f\n\r", q[0]);
worasuchad 1:5609c1795245 335 //pc.printf("q1 = %f\n\r", q[1]);
worasuchad 1:5609c1795245 336 //pc.printf("q2 = %f\n\r", q[2]);
worasuchad 1:5609c1795245 337 //pc.printf("q3 = %f\n\r", q[3]);
worasuchad 1:5609c1795245 338
worasuchad 1:5609c1795245 339
worasuchad 1:5609c1795245 340 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
worasuchad 1:5609c1795245 341 // In this coordinate system, the positive z-axis is down toward Earth.
worasuchad 1:5609c1795245 342 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
worasuchad 1:5609c1795245 343 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
worasuchad 1:5609c1795245 344 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
worasuchad 1:5609c1795245 345 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
worasuchad 1:5609c1795245 346 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
worasuchad 1:5609c1795245 347 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
worasuchad 1:5609c1795245 348 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
worasuchad 1:5609c1795245 349 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
worasuchad 1:5609c1795245 350 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
worasuchad 1:5609c1795245 351 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
worasuchad 1:5609c1795245 352 pitch *= 180.0f / PI;
worasuchad 1:5609c1795245 353 yaw *= 180.0f / PI;
worasuchad 1:5609c1795245 354 yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
worasuchad 1:5609c1795245 355 roll *= 180.0f / PI;
worasuchad 1:5609c1795245 356
worasuchad 1:5609c1795245 357 pc.printf("%f %f %f %f \n\r",roll, pitch, yaw, origin);
worasuchad 1:5609c1795245 358 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
worasuchad 1:5609c1795245 359 //sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
worasuchad 1:5609c1795245 360 //lcd.printString(buffer, 0, 4);
worasuchad 1:5609c1795245 361 //sprintf(buffer, "rate = %f", (float) sumCount/sum);
worasuchad 1:5609c1795245 362 //lcd.printString(buffer, 0, 5);
worasuchad 1:5609c1795245 363
worasuchad 1:5609c1795245 364 myled= !myled;
worasuchad 1:5609c1795245 365 count = t.read_ms();
worasuchad 1:5609c1795245 366
worasuchad 1:5609c1795245 367 if(count > 1<<21)
worasuchad 1:5609c1795245 368 {
worasuchad 1:5609c1795245 369 t.start(); // start the timer over again if ~30 minutes has passed
worasuchad 1:5609c1795245 370 count = 0;
worasuchad 1:5609c1795245 371 deltat= 0;
worasuchad 1:5609c1795245 372 lastUpdate = t.read_us();
worasuchad 1:5609c1795245 373 }
worasuchad 1:5609c1795245 374 sum = 0;
worasuchad 1:5609c1795245 375 sumCount = 0;
worasuchad 1:5609c1795245 376 }
worasuchad 1:5609c1795245 377 }
Khanchana 0:812929a5d5ad 378 }