Jordan Miller / Mbed 2 deprecated PWM

Working Degree Correction Demo

Dependencies:   LSM9DS1_Library_cal TCA9548A VL53L0X mbed mbed-rtos

main.cpp@12:505684aec350, 2018-04-24 (annotated)

Committer:
jmiller322
Date:
Tue Apr 24 00:40:50 2018 +0000
Revision:
12:505684aec350
Parent:
11:8cd808f5637a 
Added newline to heading print lines in maintainHeading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmiller322 0:52b06cb96c86 1 #include "mbed.h"
jmiller322 0:52b06cb96c86 2 #include "tca9548a.h"
jmiller322 0:52b06cb96c86 3 #include "VL53L0X.h"
jmiller322 0:52b06cb96c86 4 #include "LSM9DS1.h"
jmiller322 4:051ba9cde5a6 5 #include "rtos.h"
jmiller322 0:52b06cb96c86 6 #define PI 3.14159
jmiller322 0:52b06cb96c86 7 #define DECLINATION -4.94
jmiller322 0:52b06cb96c86 8
jmiller322 10:adcd5a485093 9
jmiller322 10:adcd5a485093 10
jmiller322 10:adcd5a485093 11 DigitalOut LED(LED1);
jmiller322 0:52b06cb96c86 12 PwmOut PWM(p21); //Right Side Motor
jmiller322 0:52b06cb96c86 13 PwmOut PWM1(p22); //Left Side Motor
jmiller322 0:52b06cb96c86 14 Serial pc(USBTX, USBRX);
Joe5181 2:2a4e77f78c8a 15 TCA9548A i2c_sw(I2C_SDA, I2C_SCL); //default address 0x70 applied
Joe5181 2:2a4e77f78c8a 16 VL53L0X ToF(I2C_SDA, I2C_SCL);
jmiller322 0:52b06cb96c86 17 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); //imu
jmiller322 5:8cd61ed95e13 18 Timer t;
Joe5181 8:083571630c0c 19 Mutex serial_mutex;
jmiller322 0:52b06cb96c86 20
jmiller322 0:52b06cb96c86 21 float dutyLeft=0.130;
jmiller322 0:52b06cb96c86 22 float dutyRight=0.130;
Joe5181 8:083571630c0c 23 volatile float desHeading = 180.0;
jmiller322 0:52b06cb96c86 24 float currentDegreeHeading;
jmiller322 0:52b06cb96c86 25 float distanceAway;
Joe5181 8:083571630c0c 26 volatile bool Phase1=false;//Phase 1 means we are in recieving mode, and should go to the desired heading passed in by the PI
Joe5181 8:083571630c0c 27 volatile bool Phase2=false;//Phase 2 means we are in transmission mode, and should update our desired heading with our current heading.
Joe5181 7:c657c9c05473 28
jmiller322 0:52b06cb96c86 29
jmiller322 0:52b06cb96c86 30 float calculateHeading(float ax, float ay, float az, float mx, float my, float mz){
jmiller322 0:52b06cb96c86 31 float roll = atan2(ay, az);
jmiller322 0:52b06cb96c86 32 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jmiller322 0:52b06cb96c86 33 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jmiller322 0:52b06cb96c86 34 mx = -mx;
jmiller322 0:52b06cb96c86 35 float heading;
jmiller322 0:52b06cb96c86 36 if (my == 0.0)
jmiller322 0:52b06cb96c86 37 heading = (mx < 0.0) ? 180.0 : 0.0;
jmiller322 0:52b06cb96c86 38 else
jmiller322 0:52b06cb96c86 39 heading = atan2(mx, my)*360.0/(2.0*PI);
jmiller322 0:52b06cb96c86 40 //pc.printf("heading atan=%f \n\r",heading);
jmiller322 0:52b06cb96c86 41 heading -= DECLINATION; //correct for geo location
jmiller322 0:52b06cb96c86 42 if(heading>180.0) heading = heading - 360.0;
jmiller322 0:52b06cb96c86 43 else if(heading<-180.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 44 else if(heading<0.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 45
jmiller322 0:52b06cb96c86 46 pitch *= 180.0 / PI;
jmiller322 0:52b06cb96c86 47 roll *= 180.0 / PI;
jmiller322 0:52b06cb96c86 48
jmiller322 0:52b06cb96c86 49 return heading;
jmiller322 0:52b06cb96c86 50 }
jmiller322 0:52b06cb96c86 51
jmiller322 0:52b06cb96c86 52
jmiller322 0:52b06cb96c86 53 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
jmiller322 0:52b06cb96c86 54 {
jmiller322 0:52b06cb96c86 55 float roll = atan2(ay, az);
jmiller322 0:52b06cb96c86 56 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
jmiller322 0:52b06cb96c86 57 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
jmiller322 0:52b06cb96c86 58 mx = -mx;
jmiller322 0:52b06cb96c86 59 float heading;
jmiller322 0:52b06cb96c86 60 if (my == 0.0)
jmiller322 0:52b06cb96c86 61 heading = (mx < 0.0) ? 180.0 : 0.0;
jmiller322 0:52b06cb96c86 62 else
jmiller322 0:52b06cb96c86 63 heading = atan2(mx, my)*360.0/(2.0*PI);
jmiller322 0:52b06cb96c86 64 heading -= DECLINATION; //correct for geo location
jmiller322 0:52b06cb96c86 65 if(heading>180.0) heading = heading - 360.0;
jmiller322 0:52b06cb96c86 66 else if(heading<-180.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 67 else if(heading<0.0) heading = 360.0 + heading;
jmiller322 0:52b06cb96c86 68
jmiller322 0:52b06cb96c86 69 pitch *= 180.0 / PI;
jmiller322 0:52b06cb96c86 70 roll *= 180.0 / PI;
jmiller322 0:52b06cb96c86 71
Joe5181 9:8c4da2b8dfbc 72 //pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
Joe5181 9:8c4da2b8dfbc 73 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
jmiller322 0:52b06cb96c86 74 }
jmiller322 0:52b06cb96c86 75
jmiller322 0:52b06cb96c86 76 void calibrateIMU()
jmiller322 0:52b06cb96c86 77 {
jmiller322 0:52b06cb96c86 78 IMU.begin();
jmiller322 0:52b06cb96c86 79 if (!IMU.begin()) {
Joe5181 9:8c4da2b8dfbc 80 ///pc.printf("Failed to communicate with LSM9DS1.\n");
jmiller322 0:52b06cb96c86 81 }
jmiller322 10:adcd5a485093 82 LED = 1;
jmiller322 0:52b06cb96c86 83 IMU.calibrate(1);
jmiller322 10:adcd5a485093 84 IMU.calibrateMag(0);
jmiller322 10:adcd5a485093 85 LED = 0;
jmiller322 0:52b06cb96c86 86 return;
jmiller322 0:52b06cb96c86 87 }
jmiller322 0:52b06cb96c86 88
jmiller322 0:52b06cb96c86 89 void initToF ()
jmiller322 0:52b06cb96c86 90 {
jmiller322 0:52b06cb96c86 91 // By default TCA9548A performs a power on reset and all downstream ports are deselected
Joe5181 2:2a4e77f78c8a 92 for(int i=0;i<4;++i){
Joe5181 9:8c4da2b8dfbc 93 //printf(" initializing Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 94 i2c_sw.select(i); // select the channel 0
Joe5181 2:2a4e77f78c8a 95 ToF.init(false);
Joe5181 9:8c4da2b8dfbc 96 //printf("finished initializing Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 97 ToF.startContinuous();
Joe5181 9:8c4da2b8dfbc 98 //printf("finished starting continous readings for Tof %d\n",i);
Joe5181 2:2a4e77f78c8a 99 }
Joe5181 2:2a4e77f78c8a 100 i2c_sw.select(0);
jmiller322 0:52b06cb96c86 101 }
jmiller322 0:52b06cb96c86 102
jmiller322 0:52b06cb96c86 103 void readToF()
jmiller322 0:52b06cb96c86 104 {
Joe5181 2:2a4e77f78c8a 105 printf("Tof 1:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 106 i2c_sw.select(1);
Joe5181 2:2a4e77f78c8a 107 printf("Tof 2:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 108 i2c_sw.select(2);
Joe5181 2:2a4e77f78c8a 109 printf("Tof 3:%d mm \t",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 110 i2c_sw.select(3);
Joe5181 2:2a4e77f78c8a 111 printf("Tof 4:%d mm \n",ToF.readRangeContinuousMillimeters());
Joe5181 2:2a4e77f78c8a 112 i2c_sw.select(0);
Joe5181 2:2a4e77f78c8a 113 wait( 0.1 );
jmiller322 0:52b06cb96c86 114 }
jmiller322 0:52b06cb96c86 115
jmiller322 0:52b06cb96c86 116 void readIMU()
jmiller322 0:52b06cb96c86 117 {
jmiller322 0:52b06cb96c86 118 while(1){
jmiller322 0:52b06cb96c86 119 while(!IMU.tempAvailable());
jmiller322 0:52b06cb96c86 120 IMU.readTemp();
jmiller322 0:52b06cb96c86 121 while(!IMU.magAvailable(X_AXIS));
jmiller322 0:52b06cb96c86 122 IMU.readMag();
jmiller322 0:52b06cb96c86 123 while(!IMU.accelAvailable());
jmiller322 0:52b06cb96c86 124 IMU.readAccel();
jmiller322 0:52b06cb96c86 125 while(!IMU.gyroAvailable());
jmiller322 0:52b06cb96c86 126 IMU.readGyro();
jmiller322 0:52b06cb96c86 127 //pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
jmiller322 0:52b06cb96c86 128 //pc.printf(" X axis Y axis Z axis\n\r");
jmiller322 0:52b06cb96c86 129 //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
jmiller322 0:52b06cb96c86 130 //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
Joe5181 9:8c4da2b8dfbc 131 //pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jmiller322 0:52b06cb96c86 132 printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jmiller322 0:52b06cb96c86 133 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
jmiller322 0:52b06cb96c86 134 //myled = 1;
jmiller322 0:52b06cb96c86 135 wait(0.25);
jmiller322 0:52b06cb96c86 136 //myled = 0;
jmiller322 0:52b06cb96c86 137 wait(0.25);
jmiller322 0:52b06cb96c86 138 }
jmiller322 0:52b06cb96c86 139 }
jmiller322 0:52b06cb96c86 140
jmiller322 0:52b06cb96c86 141 void turnRight()
jmiller322 0:52b06cb96c86 142 {
jmiller322 6:8e0f047c182d 143 dutyLeft = 0.148;
jmiller322 0:52b06cb96c86 144 dutyRight = 0.05; //turn right
jmiller322 0:52b06cb96c86 145 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 146 PWM1.write(dutyLeft);
Joe5181 9:8c4da2b8dfbc 147 //pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 148 }
jmiller322 0:52b06cb96c86 149
jmiller322 0:52b06cb96c86 150 void turnLeft()
jmiller322 0:52b06cb96c86 151 {
jmiller322 6:8e0f047c182d 152 dutyRight = 0.148; //increase right motor speed
jmiller322 0:52b06cb96c86 153 dutyLeft = 0.05; //Stop left motor
jmiller322 0:52b06cb96c86 154 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 155 PWM1.write(dutyLeft);
Joe5181 9:8c4da2b8dfbc 156 //pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 157 }
jmiller322 0:52b06cb96c86 158
jmiller322 0:52b06cb96c86 159 void goForward()
jmiller322 0:52b06cb96c86 160 {
jmiller322 0:52b06cb96c86 161 if(dutyRight < dutyLeft) {
jmiller322 0:52b06cb96c86 162 dutyLeft = dutyRight;
jmiller322 0:52b06cb96c86 163 }
jmiller322 0:52b06cb96c86 164 if(dutyLeft < dutyRight) {
jmiller322 0:52b06cb96c86 165 dutyRight = dutyLeft;
jmiller322 0:52b06cb96c86 166 }
jmiller322 6:8e0f047c182d 167 dutyRight = 0.148;
jmiller322 6:8e0f047c182d 168 dutyLeft = 0.148;
jmiller322 0:52b06cb96c86 169 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 170 PWM1.write(dutyLeft);
Joe5181 9:8c4da2b8dfbc 171 //pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 172 }
jmiller322 0:52b06cb96c86 173
jmiller322 0:52b06cb96c86 174 void slowDown()
jmiller322 0:52b06cb96c86 175 {
jmiller322 0:52b06cb96c86 176 if(dutyRight >=0.130) {
jmiller322 0:52b06cb96c86 177 dutyRight -= 0.001;
jmiller322 0:52b06cb96c86 178 }
jmiller322 0:52b06cb96c86 179 if(dutyLeft >=0.130) {
jmiller322 0:52b06cb96c86 180 dutyLeft -= 0.001;
jmiller322 0:52b06cb96c86 181 }
jmiller322 0:52b06cb96c86 182 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 183 PWM1.write(dutyLeft);
Joe5181 9:8c4da2b8dfbc 184 //pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 185 }
jmiller322 0:52b06cb96c86 186
jmiller322 0:52b06cb96c86 187 void stop()
jmiller322 0:52b06cb96c86 188 {
jmiller322 5:8cd61ed95e13 189 dutyLeft = dutyRight = 0.05;
jmiller322 0:52b06cb96c86 190 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 191 PWM1.write(dutyLeft); //stop motor
Joe5181 9:8c4da2b8dfbc 192 //pc.printf("Duty Cycle Left: %f\t Duty Cycle Right: %f\r\n", dutyLeft, dutyRight);
jmiller322 0:52b06cb96c86 193 }
jmiller322 0:52b06cb96c86 194
jmiller322 0:52b06cb96c86 195 void steady()
jmiller322 0:52b06cb96c86 196 {
jmiller322 0:52b06cb96c86 197 PWM.write(dutyRight);
jmiller322 0:52b06cb96c86 198 PWM1.write(dutyLeft);
jmiller322 0:52b06cb96c86 199 }
jmiller322 0:52b06cb96c86 200
jmiller322 0:52b06cb96c86 201 void startupESC()
jmiller322 0:52b06cb96c86 202 {
jmiller322 0:52b06cb96c86 203 PWM.write(0.01);
jmiller322 0:52b06cb96c86 204 PWM1.write(0.01);
jmiller322 0:52b06cb96c86 205 wait(0.5);
jmiller322 0:52b06cb96c86 206 PWM.write(1.0);
jmiller322 0:52b06cb96c86 207 PWM1.write(1.0);
jmiller322 0:52b06cb96c86 208 wait(8);
jmiller322 0:52b06cb96c86 209 PWM.write(0.01);
jmiller322 0:52b06cb96c86 210 PWM1.write(0.01);
jmiller322 0:52b06cb96c86 211 wait(8);
jmiller322 0:52b06cb96c86 212 }
jmiller322 0:52b06cb96c86 213
jmiller322 0:52b06cb96c86 214 void maintainHeading(float desiredHeading){
jmiller322 0:52b06cb96c86 215 while(!IMU.tempAvailable());
jmiller322 0:52b06cb96c86 216 IMU.readTemp();
jmiller322 0:52b06cb96c86 217 while(!IMU.magAvailable(X_AXIS));
jmiller322 0:52b06cb96c86 218 IMU.readMag();
jmiller322 0:52b06cb96c86 219 while(!IMU.accelAvailable());
jmiller322 0:52b06cb96c86 220 IMU.readAccel();
jmiller322 0:52b06cb96c86 221 while(!IMU.gyroAvailable());
jmiller322 0:52b06cb96c86 222 IMU.readGyro();
jmiller322 0:52b06cb96c86 223 currentDegreeHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
jmiller322 0:52b06cb96c86 224 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
Joe5181 9:8c4da2b8dfbc 225 currentDegreeHeading=fabs(currentDegreeHeading);
Joe5181 9:8c4da2b8dfbc 226 serial_mutex.lock();
jmiller322 12:505684aec350 227 pc.printf("%d\n",(int)currentDegreeHeading);
Joe5181 9:8c4da2b8dfbc 228 serial_mutex.unlock();
Joe5181 7:c657c9c05473 229 if(Phase2==true){
Joe5181 7:c657c9c05473 230 stop();
Joe5181 7:c657c9c05473 231 return;
Joe5181 7:c657c9c05473 232 }
Joe5181 9:8c4da2b8dfbc 233
Joe5181 9:8c4da2b8dfbc 234 //pc.printf("heading: %f", currentDegreeHeading);
jmiller322 0:52b06cb96c86 235 currentDegreeHeading = fmod(((double)currentDegreeHeading-desiredHeading),360.0);
jmiller322 0:52b06cb96c86 236 if(currentDegreeHeading<0.0){currentDegreeHeading=360.0-fabs(currentDegreeHeading);}
Joe5181 9:8c4da2b8dfbc 237 //pc.printf(" heading: %f\n", currentDegreeHeading);
jmiller322 0:52b06cb96c86 238
jmiller322 0:52b06cb96c86 239 if(currentDegreeHeading < 180.0 && currentDegreeHeading > 10.0) {
jmiller322 0:52b06cb96c86 240 turnRight();
jmiller322 0:52b06cb96c86 241 } else if(currentDegreeHeading > 180.0 && currentDegreeHeading < 350.0) {
jmiller322 0:52b06cb96c86 242 turnLeft();
jmiller322 0:52b06cb96c86 243 } else {
jmiller322 5:8cd61ed95e13 244 stop();
jmiller322 0:52b06cb96c86 245 }
Joe5181 9:8c4da2b8dfbc 246
jmiller322 0:52b06cb96c86 247 }
jmiller322 0:52b06cb96c86 248
jmiller322 1:bae4f01ae25c 249 void serialRx(){
jmiller322 1:bae4f01ae25c 250 char inp = pc.getc();
jmiller322 1:bae4f01ae25c 251 ///// GO FORWARD /////
jmiller322 1:bae4f01ae25c 252 if(inp == 'w') {
jmiller322 1:bae4f01ae25c 253 //goForward();
jmiller322 1:bae4f01ae25c 254 desHeading += 15.0;
jmiller322 1:bae4f01ae25c 255 }
jmiller322 1:bae4f01ae25c 256 ///// SLOW DOWN /////
jmiller322 1:bae4f01ae25c 257 else if(inp == 's') {
jmiller322 1:bae4f01ae25c 258 //slowDown();
jmiller322 1:bae4f01ae25c 259 desHeading -= 15.0;
jmiller322 1:bae4f01ae25c 260 }
jmiller322 1:bae4f01ae25c 261 desHeading = fmod(desHeading, 360);
jmiller322 1:bae4f01ae25c 262 }
jmiller322 1:bae4f01ae25c 263
Joe5181 2:2a4e77f78c8a 264 void init_motors(){
jmiller322 0:52b06cb96c86 265 PWM.period(0.001);
jmiller322 0:52b06cb96c86 266 PWM1.period(0.001);
jmiller322 0:52b06cb96c86 267 startupESC();
Joe5181 9:8c4da2b8dfbc 268 //pc.printf("finished ecs startup");
Joe5181 2:2a4e77f78c8a 269 }
Joe5181 2:2a4e77f78c8a 270 void overall_init(){
jmiller322 5:8cd61ed95e13 271
jmiller322 5:8cd61ed95e13 272 init_motors();
jmiller322 5:8cd61ed95e13 273 calibrateIMU();
Joe5181 9:8c4da2b8dfbc 274 //printf("finished imu cal\n");
jmiller322 6:8e0f047c182d 275 currentDegreeHeading = calculateHeading(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
Joe5181 2:2a4e77f78c8a 276 IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
Joe5181 2:2a4e77f78c8a 277 }
Joe5181 2:2a4e77f78c8a 278
Joe5181 2:2a4e77f78c8a 279 void take_input(){
Joe5181 2:2a4e77f78c8a 280 char inp = pc.getc();
jmiller322 0:52b06cb96c86 281 ///// GO FORWARD /////
jmiller322 0:52b06cb96c86 282 if(inp == 'w' && dutyRight <= 0.230 && dutyLeft <= 0.230) {
jmiller322 0:52b06cb96c86 283 goForward();
jmiller322 0:52b06cb96c86 284 }
jmiller322 0:52b06cb96c86 285 ///// SLOW DOWN /////
jmiller322 0:52b06cb96c86 286 else if(inp == 's') {
jmiller322 0:52b06cb96c86 287 slowDown();
jmiller322 0:52b06cb96c86 288 }
jmiller322 0:52b06cb96c86 289 else if(inp == 't') {
jmiller322 0:52b06cb96c86 290 readToF();
jmiller322 0:52b06cb96c86 291 }
jmiller322 0:52b06cb96c86 292 else if(inp == 'i') {
jmiller322 0:52b06cb96c86 293 //readIMU();
jmiller322 0:52b06cb96c86 294 }
jmiller322 0:52b06cb96c86 295 ///// TURN LEFT /////
jmiller322 0:52b06cb96c86 296 else if(inp == 'a' && dutyRight <= 0.230) {
jmiller322 0:52b06cb96c86 297 turnLeft();
jmiller322 0:52b06cb96c86 298 }
jmiller322 0:52b06cb96c86 299 ///// TURN RIGHT /////
jmiller322 0:52b06cb96c86 300 else if(inp == 'd' && dutyLeft <= 0.230) {
jmiller322 0:52b06cb96c86 301 turnRight();
jmiller322 0:52b06cb96c86 302 }
jmiller322 0:52b06cb96c86 303 ///// STOP /////
jmiller322 0:52b06cb96c86 304 else if(inp == 'x') {
jmiller322 0:52b06cb96c86 305 stop();
jmiller322 0:52b06cb96c86 306 }
jmiller322 0:52b06cb96c86 307
jmiller322 0:52b06cb96c86 308 ///// STEADY PACE /////
Joe5181 2:2a4e77f78c8a 309 else {
jmiller322 0:52b06cb96c86 310
Joe5181 2:2a4e77f78c8a 311 steady();
Joe5181 2:2a4e77f78c8a 312 }
jmiller322 0:52b06cb96c86 313 }
Joe5181 2:2a4e77f78c8a 314
jmiller322 4:051ba9cde5a6 315 void dev_recv(){
jmiller322 4:051ba9cde5a6 316 int heading;
jmiller322 4:051ba9cde5a6 317 char headingstr[4];
jmiller322 4:051ba9cde5a6 318
jmiller322 4:051ba9cde5a6 319 while(1){
jmiller322 4:051ba9cde5a6 320 while(pc.readable()) {
Joe5181 8:083571630c0c 321 //pc.scanf("%s", &headingstr);
Joe5181 8:083571630c0c 322 serial_mutex.lock();
Joe5181 8:083571630c0c 323 pc.gets(headingstr, 4);
jmiller322 4:051ba9cde5a6 324 heading=atoi(headingstr);
Joe5181 7:c657c9c05473 325 if(heading==361){
Joe5181 9:8c4da2b8dfbc 326 //pc.printf("going into phase 1 \n");
Joe5181 7:c657c9c05473 327 Phase1=true;
Joe5181 7:c657c9c05473 328 Phase2=false;
Joe5181 8:083571630c0c 329 heading=0;
Joe5181 8:083571630c0c 330 serial_mutex.unlock();
Joe5181 7:c657c9c05473 331 continue;
Joe5181 7:c657c9c05473 332 }
Joe5181 7:c657c9c05473 333 if(heading==362){
Joe5181 9:8c4da2b8dfbc 334 //pc.printf("going into phase 2 \n");
Joe5181 7:c657c9c05473 335 Phase1=false;
Joe5181 7:c657c9c05473 336 Phase2=true;
Joe5181 8:083571630c0c 337 heading=0;
Joe5181 8:083571630c0c 338 serial_mutex.unlock();
Joe5181 7:c657c9c05473 339 continue;
Joe5181 7:c657c9c05473 340 }
Joe5181 9:8c4da2b8dfbc 341 //pc.printf("The recieved heading is %d\n",heading);
Joe5181 7:c657c9c05473 342 desHeading=heading;
jmiller322 4:051ba9cde5a6 343 heading=0;
Joe5181 8:083571630c0c 344 serial_mutex.unlock();
jmiller322 4:051ba9cde5a6 345 }
jmiller322 4:051ba9cde5a6 346
jmiller322 4:051ba9cde5a6 347 }
jmiller322 4:051ba9cde5a6 348 }
Joe5181 9:8c4da2b8dfbc 349 void sendHeading(){
Joe5181 9:8c4da2b8dfbc 350 while(1){
Joe5181 9:8c4da2b8dfbc 351 serial_mutex.lock();
Joe5181 9:8c4da2b8dfbc 352
Joe5181 9:8c4da2b8dfbc 353 serial_mutex.unlock();
Joe5181 9:8c4da2b8dfbc 354 }
Joe5181 9:8c4da2b8dfbc 355 }
jmiller322 4:051ba9cde5a6 356
Joe5181 2:2a4e77f78c8a 357 int main(){
Joe5181 8:083571630c0c 358 // pc.printf("before wait function\n");
Joe5181 8:083571630c0c 359 // wait(10);
Joe5181 8:083571630c0c 360 // pc.printf("before overall init function\n");
Joe5181 8:083571630c0c 361 // overall_init();
jmiller322 4:051ba9cde5a6 362 //initToF();
jmiller322 11:8cd808f5637a 363 LED = 0;
Joe5181 8:083571630c0c 364 calibrateIMU();
jmiller322 5:8cd61ed95e13 365 //overallInit();
Joe5181 9:8c4da2b8dfbc 366 //pc.printf("before entering the while loop\n");
Joe5181 8:083571630c0c 367 Thread t1(dev_recv);
Joe5181 9:8c4da2b8dfbc 368 Thread t2(sendHeading);
Joe5181 2:2a4e77f78c8a 369 while(1) {
Joe5181 8:083571630c0c 370
jmiller322 5:8cd61ed95e13 371 //dev_recv();
jmiller322 4:051ba9cde5a6 372 //readToF();
jmiller322 5:8cd61ed95e13 373 maintainHeading(desHeading);
Joe5181 2:2a4e77f78c8a 374 //take_input();
Joe5181 2:2a4e77f78c8a 375 }
jmiller322 5:8cd61ed95e13 376 stop();
jmiller322 5:8cd61ed95e13 377 while(1){
jmiller322 5:8cd61ed95e13 378 sleep();
jmiller322 5:8cd61ed95e13 379 }
Joe5181 2:2a4e77f78c8a 380 }