Saul Crumpton / Mbed 2 deprecated 4012Code

forking so that I can submit a pull request to jmiller322

Dependencies:   LSM9DS1_Library_cal TCA9548A VL53L0X mbed

Fork of PWM by Jordan Miller

main.cpp@0:52b06cb96c86, 2018-04-15 (annotated)

Committer:
jmiller322
Date:
Sun Apr 15 02:10:41 2018 +0000
Revision:
0:52b06cb96c86
Child:
1:bae4f01ae25c
working IMU heading + Motor correction demo

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