Edward Leland / Mbed 2 deprecated app-board-Bubble-Level

Drone code for Prof. Coode

Dependencies:   C12832_lcd FatFileSystemCpp MMA7660 mbed

Fork of app-board-Bubble-Level by jim hamblen

main.cpp@3:6dae4f871cdc, 2015-09-10 (annotated)

Committer:
ecleland
Date:
Thu Sep 10 18:22:29 2015 +0000
Revision:
3:6dae4f871cdc
Parent:
2:2fb847807890 
Drone Code for Prof. Cooke

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ecleland 3:6dae4f871cdc 1
Sissors 0:bd0546063b0a 2 #include "mbed.h"
Sissors 0:bd0546063b0a 3 #include "MMA7660.h"
4180_1 1:876f52a697c1 4 #include "C12832_lcd.h"
ecleland 3:6dae4f871cdc 5 #include "MSCFileSystem.h"
ecleland 3:6dae4f871cdc 6 #include <string>
ecleland 3:6dae4f871cdc 7
ecleland 2:2fb847807890 8
ecleland 2:2fb847807890 9 Serial pc(USBTX, USBRX); // tx, rx
Sissors 0:bd0546063b0a 10
4180_1 1:876f52a697c1 11 C12832_LCD lcd; //On board LCD display
4180_1 1:876f52a697c1 12 MMA7660 MMA(p28, p27); //I2C Accelerometer
4180_1 1:876f52a697c1 13 DigitalOut connectionLed(LED1);//Accel OK LED
Sissors 0:bd0546063b0a 14
ecleland 3:6dae4f871cdc 15 MSCFileSystem fs("fs");
ecleland 3:6dae4f871cdc 16 FILE *fp = fopen("/fs/movement.txt","w");
ecleland 3:6dae4f871cdc 17
ecleland 3:6dae4f871cdc 18 int numMovements = 0;
ecleland 3:6dae4f871cdc 19 bool rep = false;
ecleland 2:2fb847807890 20
ecleland 3:6dae4f871cdc 21 DigitalIn up(p15);
ecleland 3:6dae4f871cdc 22 DigitalIn down(p12);
ecleland 3:6dae4f871cdc 23 DigitalIn left(p13);
ecleland 3:6dae4f871cdc 24 DigitalIn right(p16);
ecleland 3:6dae4f871cdc 25 DigitalIn pressed(p14);
ecleland 2:2fb847807890 26
ecleland 3:6dae4f871cdc 27
ecleland 3:6dae4f871cdc 28
ecleland 2:2fb847807890 29 DigitalOut strafe(p21);
ecleland 2:2fb847807890 30 DigitalOut forback(p22);
ecleland 2:2fb847807890 31 DigitalOut updown(p23);
ecleland 2:2fb847807890 32 DigitalOut rlturn(p24);
ecleland 2:2fb847807890 33 AnalogOut output(p18);
ecleland 2:2fb847807890 34
ecleland 2:2fb847807890 35 float StrafeVM = 0.48638;
ecleland 2:2fb847807890 36 float ForBackVM = .508172;
ecleland 2:2fb847807890 37 float UpDownVM = .50424;
ecleland 2:2fb847807890 38 float RLTurnVM = .508475;
ecleland 2:2fb847807890 39
ecleland 2:2fb847807890 40 float step = .001;
ecleland 2:2fb847807890 41
ecleland 3:6dae4f871cdc 42
ecleland 3:6dae4f871cdc 43
ecleland 2:2fb847807890 44 void NoOut()
ecleland 2:2fb847807890 45 {
ecleland 2:2fb847807890 46 strafe = 1;
ecleland 2:2fb847807890 47 forback = 1;
ecleland 2:2fb847807890 48 updown = 1;
ecleland 2:2fb847807890 49 rlturn = 1;
ecleland 2:2fb847807890 50 }
ecleland 2:2fb847807890 51
ecleland 2:2fb847807890 52 void SET(string movement, float Out)
ecleland 3:6dae4f871cdc 53 {
ecleland 3:6dae4f871cdc 54
ecleland 2:2fb847807890 55 NoOut();
ecleland 2:2fb847807890 56
ecleland 2:2fb847807890 57 output = Out;
ecleland 2:2fb847807890 58
ecleland 2:2fb847807890 59 if (movement == "s") {
ecleland 2:2fb847807890 60 strafe = 0;
ecleland 2:2fb847807890 61 }
ecleland 2:2fb847807890 62
ecleland 2:2fb847807890 63 if (movement == "fb") {
ecleland 2:2fb847807890 64 forback = 0;
ecleland 2:2fb847807890 65 }
ecleland 2:2fb847807890 66
ecleland 2:2fb847807890 67 if (movement == "ud") {
ecleland 2:2fb847807890 68 updown = 0;
ecleland 2:2fb847807890 69 }
ecleland 2:2fb847807890 70
ecleland 2:2fb847807890 71 if (movement == "rl") {
ecleland 2:2fb847807890 72 rlturn = 0;
ecleland 2:2fb847807890 73 }
ecleland 3:6dae4f871cdc 74
ecleland 3:6dae4f871cdc 75
ecleland 3:6dae4f871cdc 76 wait(.001);
ecleland 2:2fb847807890 77
ecleland 3:6dae4f871cdc 78 if (!rep){
ecleland 3:6dae4f871cdc 79 fprintf(fp,"%s %f ", (string)movement, (float)Out);
ecleland 3:6dae4f871cdc 80 numMovements ++;
ecleland 3:6dae4f871cdc 81 }
ecleland 3:6dae4f871cdc 82 //fprintf(fp,"%f\r\n", (float)Out);
ecleland 3:6dae4f871cdc 83
ecleland 3:6dae4f871cdc 84
ecleland 2:2fb847807890 85
ecleland 2:2fb847807890 86 NoOut();
ecleland 3:6dae4f871cdc 87
ecleland 3:6dae4f871cdc 88
ecleland 2:2fb847807890 89 }
ecleland 2:2fb847807890 90
ecleland 2:2fb847807890 91 //latch chip drifts voltage up pretty quickly beyond the limits of the phantom if we reset the values though all is good.
ecleland 2:2fb847807890 92 void REFRESH()
ecleland 2:2fb847807890 93 {
ecleland 2:2fb847807890 94 SET("s", StrafeVM);
ecleland 2:2fb847807890 95 SET("fb", ForBackVM);
ecleland 2:2fb847807890 96 SET("ud", UpDownVM);
ecleland 2:2fb847807890 97 SET("rl", RLTurnVM);
ecleland 2:2fb847807890 98 }
ecleland 2:2fb847807890 99
ecleland 2:2fb847807890 100 void IDLE()
ecleland 2:2fb847807890 101 {
ecleland 2:2fb847807890 102 SET("s", 0.48638);
ecleland 2:2fb847807890 103 SET("fb", 0.508172);
ecleland 2:2fb847807890 104 SET("ud", 0.50424);
ecleland 2:2fb847807890 105 SET("rl", 0.508475);
ecleland 2:2fb847807890 106 }
ecleland 2:2fb847807890 107
ecleland 2:2fb847807890 108 void START()
ecleland 2:2fb847807890 109 {
ecleland 2:2fb847807890 110 SET("s", 0.575379);
ecleland 2:2fb847807890 111 SET("fb", 0.592171);
ecleland 2:2fb847807890 112 SET("ud", 0.421241);
ecleland 2:2fb847807890 113 SET("rl", 0.589474);
ecleland 3:6dae4f871cdc 114 wait(2);
ecleland 3:6dae4f871cdc 115 }
ecleland 3:6dae4f871cdc 116
ecleland 3:6dae4f871cdc 117 void Repeat() {
ecleland 3:6dae4f871cdc 118 fclose(fp);
ecleland 3:6dae4f871cdc 119
ecleland 3:6dae4f871cdc 120 rep = true;
ecleland 3:6dae4f871cdc 121
ecleland 3:6dae4f871cdc 122 string movement;
ecleland 3:6dae4f871cdc 123 float Out;
ecleland 3:6dae4f871cdc 124
ecleland 3:6dae4f871cdc 125
ecleland 3:6dae4f871cdc 126 FILE *directions = fopen("/fs/movement.txt","r");
ecleland 3:6dae4f871cdc 127 if (directions == NULL) {
ecleland 3:6dae4f871cdc 128 lcd.printf("Cannot Open Movement Log For Repeat");
ecleland 3:6dae4f871cdc 129 }
ecleland 3:6dae4f871cdc 130
ecleland 3:6dae4f871cdc 131 pc.printf("\r\n%d movements\r\n", numMovements);
ecleland 3:6dae4f871cdc 132
ecleland 3:6dae4f871cdc 133 for (int i = 0; i < numMovements; i++) {
ecleland 3:6dae4f871cdc 134 fscanf(directions, "%s %f ", movement, &Out);
ecleland 3:6dae4f871cdc 135
ecleland 3:6dae4f871cdc 136
ecleland 3:6dae4f871cdc 137 SET(movement, Out);
ecleland 3:6dae4f871cdc 138 pc.printf("%d %s %f\r\n", i, movement, Out);
ecleland 3:6dae4f871cdc 139
ecleland 3:6dae4f871cdc 140
ecleland 3:6dae4f871cdc 141 }
ecleland 3:6dae4f871cdc 142
ecleland 3:6dae4f871cdc 143 fclose(directions);
ecleland 3:6dae4f871cdc 144 }
ecleland 3:6dae4f871cdc 145
ecleland 3:6dae4f871cdc 146 void repeatMenu()
ecleland 3:6dae4f871cdc 147 {
ecleland 3:6dae4f871cdc 148 wait(1);
ecleland 3:6dae4f871cdc 149 bool rep = true;
ecleland 3:6dae4f871cdc 150
ecleland 3:6dae4f871cdc 151 lcd.cls();
ecleland 3:6dae4f871cdc 152 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 153 lcd.printf("Retrace Flight Path?\nYES\nNO");
ecleland 3:6dae4f871cdc 154 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 155 lcd.printf("<");
ecleland 3:6dae4f871cdc 156
ecleland 3:6dae4f871cdc 157 while(1) {
ecleland 3:6dae4f871cdc 158
ecleland 3:6dae4f871cdc 159 if(up) {
ecleland 3:6dae4f871cdc 160 rep = true;
ecleland 3:6dae4f871cdc 161 lcd.cls();
ecleland 3:6dae4f871cdc 162 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 163 lcd.printf("Retrace Flight Path?\nYES\nNO");
ecleland 3:6dae4f871cdc 164 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 165 lcd.printf("<");
ecleland 3:6dae4f871cdc 166 }
ecleland 3:6dae4f871cdc 167
ecleland 3:6dae4f871cdc 168 if(down) {
ecleland 3:6dae4f871cdc 169 rep = false;
ecleland 3:6dae4f871cdc 170 lcd.cls();
ecleland 3:6dae4f871cdc 171 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 172 lcd.printf("Retrace Flight Path?\nYES\nNO");
ecleland 3:6dae4f871cdc 173 lcd.locate(20,18);
ecleland 3:6dae4f871cdc 174 lcd.printf("<");
ecleland 3:6dae4f871cdc 175 }
ecleland 3:6dae4f871cdc 176
ecleland 3:6dae4f871cdc 177 if(pressed) {
ecleland 3:6dae4f871cdc 178 lcd.cls();
ecleland 3:6dae4f871cdc 179 if(rep) {
ecleland 3:6dae4f871cdc 180 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 181 lcd.printf("Place Drone at Starting\nPosition and Press Joystick\nto Begin.");
ecleland 3:6dae4f871cdc 182
ecleland 3:6dae4f871cdc 183 while(!pressed){
ecleland 3:6dae4f871cdc 184 }
ecleland 3:6dae4f871cdc 185 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 186 lcd.cls();
ecleland 3:6dae4f871cdc 187 lcd.printf("Beginning Retrace");
ecleland 3:6dae4f871cdc 188 Repeat();
ecleland 3:6dae4f871cdc 189 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 190 lcd.cls();
ecleland 3:6dae4f871cdc 191 lcd.printf("Retrace Finished");
ecleland 3:6dae4f871cdc 192 wait(2);
ecleland 3:6dae4f871cdc 193 return;
ecleland 3:6dae4f871cdc 194 }
ecleland 3:6dae4f871cdc 195 lcd.printf("Drone off.");
ecleland 3:6dae4f871cdc 196 return;
ecleland 3:6dae4f871cdc 197 }
ecleland 3:6dae4f871cdc 198
ecleland 3:6dae4f871cdc 199
ecleland 3:6dae4f871cdc 200 }
ecleland 3:6dae4f871cdc 201 }
ecleland 3:6dae4f871cdc 202
ecleland 3:6dae4f871cdc 203
ecleland 3:6dae4f871cdc 204
ecleland 3:6dae4f871cdc 205 int startupMenu()
ecleland 3:6dae4f871cdc 206 {
ecleland 3:6dae4f871cdc 207 if (fp == NULL) {
ecleland 3:6dae4f871cdc 208 lcd.cls();
ecleland 3:6dae4f871cdc 209 lcd.locate(0,3);
ecleland 3:6dae4f871cdc 210 lcd.printf("Could Not Connect to USB FLash Drive.");
ecleland 3:6dae4f871cdc 211 wait(2);
ecleland 3:6dae4f871cdc 212 }
ecleland 3:6dae4f871cdc 213
ecleland 3:6dae4f871cdc 214
ecleland 3:6dae4f871cdc 215 bool on = true;
ecleland 3:6dae4f871cdc 216
ecleland 3:6dae4f871cdc 217
ecleland 3:6dae4f871cdc 218
ecleland 3:6dae4f871cdc 219 lcd.cls();
ecleland 3:6dae4f871cdc 220 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 221 lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
ecleland 3:6dae4f871cdc 222 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 223 lcd.printf("<");
ecleland 3:6dae4f871cdc 224
ecleland 3:6dae4f871cdc 225 while(1) {
ecleland 3:6dae4f871cdc 226
ecleland 3:6dae4f871cdc 227 if(up) {
ecleland 3:6dae4f871cdc 228 on = true;
ecleland 3:6dae4f871cdc 229 lcd.cls();
ecleland 3:6dae4f871cdc 230 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 231 lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
ecleland 3:6dae4f871cdc 232 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 233 lcd.printf("<");
ecleland 3:6dae4f871cdc 234 }
ecleland 3:6dae4f871cdc 235
ecleland 3:6dae4f871cdc 236 if(down) {
ecleland 3:6dae4f871cdc 237 on = false;
ecleland 3:6dae4f871cdc 238 lcd.cls();
ecleland 3:6dae4f871cdc 239 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 240 lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
ecleland 3:6dae4f871cdc 241 lcd.locate(20,18);
ecleland 3:6dae4f871cdc 242 lcd.printf("<");
ecleland 3:6dae4f871cdc 243 }
ecleland 3:6dae4f871cdc 244
ecleland 3:6dae4f871cdc 245 if(pressed) {
ecleland 3:6dae4f871cdc 246 lcd.cls();
ecleland 3:6dae4f871cdc 247 if(on) {
ecleland 3:6dae4f871cdc 248 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 249 lcd.printf("Starting Drone...");
ecleland 3:6dae4f871cdc 250 IDLE();
ecleland 3:6dae4f871cdc 251 START();
ecleland 3:6dae4f871cdc 252 IDLE();
ecleland 3:6dae4f871cdc 253 lcd.cls();
ecleland 3:6dae4f871cdc 254 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 255 lcd.printf("Drone On.");
ecleland 3:6dae4f871cdc 256 return 1;
ecleland 3:6dae4f871cdc 257 }
ecleland 3:6dae4f871cdc 258 lcd.printf("Drone off.");
ecleland 3:6dae4f871cdc 259 return 0;
ecleland 3:6dae4f871cdc 260 }
ecleland 3:6dae4f871cdc 261
ecleland 3:6dae4f871cdc 262
ecleland 3:6dae4f871cdc 263 }
ecleland 3:6dae4f871cdc 264 }
ecleland 3:6dae4f871cdc 265
ecleland 3:6dae4f871cdc 266 int turnOffMenu() {
ecleland 3:6dae4f871cdc 267
ecleland 3:6dae4f871cdc 268 IDLE();
ecleland 3:6dae4f871cdc 269 wait(1);
ecleland 3:6dae4f871cdc 270 bool off = true;
ecleland 3:6dae4f871cdc 271
ecleland 3:6dae4f871cdc 272 lcd.cls();
ecleland 3:6dae4f871cdc 273 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 274 lcd.printf("Turn Phantom 2 Drone off?\nYES\nNO");
ecleland 3:6dae4f871cdc 275 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 276 lcd.printf("<");
ecleland 3:6dae4f871cdc 277
ecleland 3:6dae4f871cdc 278 while(1) {
ecleland 3:6dae4f871cdc 279
ecleland 3:6dae4f871cdc 280 if(up) {
ecleland 3:6dae4f871cdc 281 off = true;
ecleland 3:6dae4f871cdc 282 lcd.cls();
ecleland 3:6dae4f871cdc 283 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 284 lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
ecleland 3:6dae4f871cdc 285 lcd.locate(20,9);
ecleland 3:6dae4f871cdc 286 lcd.printf("<");
ecleland 3:6dae4f871cdc 287 }
ecleland 3:6dae4f871cdc 288
ecleland 3:6dae4f871cdc 289 if(down) {
ecleland 3:6dae4f871cdc 290 off = false;
ecleland 3:6dae4f871cdc 291 lcd.cls();
ecleland 3:6dae4f871cdc 292 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 293 lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
ecleland 3:6dae4f871cdc 294 lcd.locate(20,18);
ecleland 3:6dae4f871cdc 295 lcd.printf("<");
ecleland 3:6dae4f871cdc 296 }
ecleland 3:6dae4f871cdc 297
ecleland 3:6dae4f871cdc 298 if(pressed) {
ecleland 3:6dae4f871cdc 299 lcd.cls();
ecleland 3:6dae4f871cdc 300 if(off) {
ecleland 3:6dae4f871cdc 301 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 302 lcd.printf("Turning Drone Off...");
ecleland 3:6dae4f871cdc 303
ecleland 3:6dae4f871cdc 304 SET("s", 0.48638);
ecleland 3:6dae4f871cdc 305 SET("fb", 0.508172);
ecleland 3:6dae4f871cdc 306 SET("ud", .421);
ecleland 3:6dae4f871cdc 307 SET("rl", 0.508475);
ecleland 3:6dae4f871cdc 308 wait(3);
ecleland 3:6dae4f871cdc 309 lcd.cls();
ecleland 3:6dae4f871cdc 310 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 311 lcd.printf("Drone Off.");
ecleland 3:6dae4f871cdc 312 return 0;
ecleland 3:6dae4f871cdc 313
ecleland 3:6dae4f871cdc 314 }
ecleland 3:6dae4f871cdc 315 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 316 lcd.printf("Drone On.");
ecleland 3:6dae4f871cdc 317 return 1;
ecleland 3:6dae4f871cdc 318 }
ecleland 3:6dae4f871cdc 319 }
ecleland 2:2fb847807890 320 }
ecleland 2:2fb847807890 321
4180_1 1:876f52a697c1 322 int main()
ecleland 3:6dae4f871cdc 323 {
ecleland 3:6dae4f871cdc 324 int on = startupMenu();
ecleland 3:6dae4f871cdc 325 if(on) {
ecleland 3:6dae4f871cdc 326
ecleland 2:2fb847807890 327
ecleland 3:6dae4f871cdc 328
ecleland 3:6dae4f871cdc 329
ecleland 2:2fb847807890 330
ecleland 2:2fb847807890 331 //all limits need to be investigatged as vcc is not actually +3.304V also phantom 2 voltages vary based on battery charge in controller.
ecleland 2:2fb847807890 332 while(on) {
ecleland 2:2fb847807890 333
ecleland 3:6dae4f871cdc 334
ecleland 3:6dae4f871cdc 335
ecleland 2:2fb847807890 336 if(MMA.z() < 0) {
ecleland 2:2fb847807890 337 IDLE();
ecleland 2:2fb847807890 338 START();
ecleland 2:2fb847807890 339 IDLE();
ecleland 2:2fb847807890 340 }
ecleland 2:2fb847807890 341
ecleland 3:6dae4f871cdc 342
ecleland 3:6dae4f871cdc 343 if(MMA.y() < -.6) {
ecleland 3:6dae4f871cdc 344 ForBackVM -= step;
ecleland 3:6dae4f871cdc 345 SET("fb", ForBackVM);
ecleland 3:6dae4f871cdc 346 pc.printf("%f%", ForBackVM);
ecleland 3:6dae4f871cdc 347 }
ecleland 3:6dae4f871cdc 348
ecleland 3:6dae4f871cdc 349 if(MMA.y() > .6) {
ecleland 3:6dae4f871cdc 350 ForBackVM += step;
ecleland 3:6dae4f871cdc 351 SET("fb", ForBackVM);
ecleland 3:6dae4f871cdc 352 pc.printf("%f%", ForBackVM);
ecleland 3:6dae4f871cdc 353 }
ecleland 3:6dae4f871cdc 354
ecleland 3:6dae4f871cdc 355 if(MMA.x() > .6) {
ecleland 3:6dae4f871cdc 356 StrafeVM += step;
ecleland 3:6dae4f871cdc 357 SET("s", StrafeVM);
ecleland 3:6dae4f871cdc 358 pc.printf("%f%", StrafeVM);
ecleland 3:6dae4f871cdc 359 }
ecleland 3:6dae4f871cdc 360
ecleland 3:6dae4f871cdc 361 if(MMA.x() < -.6) {
ecleland 3:6dae4f871cdc 362 StrafeVM -= step;
ecleland 3:6dae4f871cdc 363 SET("s", StrafeVM);
ecleland 3:6dae4f871cdc 364 pc.printf("%f%", StrafeVM);
ecleland 3:6dae4f871cdc 365 }
ecleland 3:6dae4f871cdc 366
ecleland 3:6dae4f871cdc 367
ecleland 3:6dae4f871cdc 368 if(pressed) {
ecleland 3:6dae4f871cdc 369 on = turnOffMenu();
ecleland 3:6dae4f871cdc 370 if(!on) {
ecleland 3:6dae4f871cdc 371 repeatMenu();
ecleland 3:6dae4f871cdc 372 }
ecleland 3:6dae4f871cdc 373
ecleland 3:6dae4f871cdc 374 }
ecleland 3:6dae4f871cdc 375
ecleland 2:2fb847807890 376 //set c to be arbitrary
ecleland 2:2fb847807890 377 char c = '?';
ecleland 2:2fb847807890 378
ecleland 2:2fb847807890 379 REFRESH();
ecleland 2:2fb847807890 380
ecleland 2:2fb847807890 381 //pc.getc() hangs (stops) the loop until a caracter is read. due to the need to
ecleland 2:2fb847807890 382 if (pc.readable()) {
ecleland 2:2fb847807890 383 c = pc.getc();
ecleland 2:2fb847807890 384 }
ecleland 2:2fb847807890 385
ecleland 2:2fb847807890 386 //strafing
ecleland 2:2fb847807890 387 if((c == 'a') && (StrafeVM < /*0.57234*/ .6)) {
ecleland 2:2fb847807890 388 StrafeVM += step;
ecleland 2:2fb847807890 389 SET("s", StrafeVM);
ecleland 2:2fb847807890 390 pc.printf("%f%", StrafeVM);
ecleland 2:2fb847807890 391 }
ecleland 2:2fb847807890 392 if((c == 'd') && (StrafeVM > 0.410714)) {
ecleland 2:2fb847807890 393 StrafeVM -= step;
ecleland 2:2fb847807890 394 SET("s", StrafeVM);
ecleland 2:2fb847807890 395 pc.printf("%f%", StrafeVM);
ecleland 2:2fb847807890 396 }
ecleland 2:2fb847807890 397
ecleland 2:2fb847807890 398 //Forward and Backwards
ecleland 2:2fb847807890 399 if((c == 'w') && (ForBackVM > 0.424031)) {
ecleland 2:2fb847807890 400 ForBackVM -= step;
ecleland 2:2fb847807890 401 SET("fb", ForBackVM);
ecleland 2:2fb847807890 402 pc.printf("%f%", ForBackVM);
ecleland 2:2fb847807890 403 }
ecleland 2:2fb847807890 404 if((c == 's') && (ForBackVM < /*0.58535*/ .6)) {
ecleland 2:2fb847807890 405 ForBackVM += step;
ecleland 2:2fb847807890 406 SET("fb", ForBackVM);
ecleland 2:2fb847807890 407 pc.printf("%f%", ForBackVM);
ecleland 2:2fb847807890 408 }
ecleland 2:2fb847807890 409
ecleland 2:2fb847807890 410 //Up and Down Issues with being off by .08v ish
ecleland 2:2fb847807890 411 if((c == 'i') && (UpDownVM < 0.58323)) {
ecleland 2:2fb847807890 412 UpDownVM += step;
ecleland 2:2fb847807890 413 SET("ud", UpDownVM);
ecleland 2:2fb847807890 414 pc.printf("%f%", UpDownVM);
ecleland 2:2fb847807890 415 }
ecleland 2:2fb847807890 416 if((c == 'k') && (UpDownVM > 0.42161)) {
ecleland 2:2fb847807890 417 UpDownVM -= step;
ecleland 2:2fb847807890 418 SET("ud", UpDownVM);
ecleland 2:2fb847807890 419 pc.printf("%f%", UpDownVM);
ecleland 2:2fb847807890 420 }
ecleland 2:2fb847807890 421
ecleland 2:2fb847807890 422 //Turning
ecleland 2:2fb847807890 423 if((c == 'j') && (RLTurnVM < 0.5905)) {
ecleland 2:2fb847807890 424 RLTurnVM += step;
ecleland 2:2fb847807890 425 SET("rl", RLTurnVM);
ecleland 2:2fb847807890 426 pc.printf("%f%", RLTurnVM);
ecleland 2:2fb847807890 427 }
ecleland 2:2fb847807890 428 if((c == 'l') && (RLTurnVM > 0.42615)) {
ecleland 2:2fb847807890 429 RLTurnVM -= step;
ecleland 2:2fb847807890 430 SET("rl", RLTurnVM);
ecleland 2:2fb847807890 431 pc.printf("%f%", RLTurnVM);
ecleland 2:2fb847807890 432 }
ecleland 2:2fb847807890 433
ecleland 2:2fb847807890 434 //emergency idle
ecleland 2:2fb847807890 435 if(c == ' ') {
ecleland 2:2fb847807890 436 IDLE();
ecleland 2:2fb847807890 437 }
ecleland 2:2fb847807890 438
ecleland 2:2fb847807890 439 //ground/turn off the drone
ecleland 2:2fb847807890 440 if(c == 'g') {
ecleland 2:2fb847807890 441 IDLE();
ecleland 2:2fb847807890 442
ecleland 2:2fb847807890 443 while(UpDownVM > 0.48) {
ecleland 2:2fb847807890 444 UpDownVM -= .01;
ecleland 2:2fb847807890 445 SET("ud", UpDownVM);
ecleland 2:2fb847807890 446 wait(0.1);
ecleland 2:2fb847807890 447 }
ecleland 2:2fb847807890 448 wait(2);
ecleland 2:2fb847807890 449
ecleland 3:6dae4f871cdc 450 on = 0;
ecleland 2:2fb847807890 451
ecleland 2:2fb847807890 452 }
ecleland 2:2fb847807890 453
ecleland 2:2fb847807890 454 }
ecleland 3:6dae4f871cdc 455 fclose(fp);
ecleland 2:2fb847807890 456
ecleland 2:2fb847807890 457 }
ecleland 2:2fb847807890 458
ecleland 3:6dae4f871cdc 459
ecleland 3:6dae4f871cdc 460 lcd.cls();
ecleland 3:6dae4f871cdc 461 lcd.locate(0,0);
ecleland 3:6dae4f871cdc 462 lcd.printf("Program Finished");
ecleland 2:2fb847807890 463 return 0;
Sissors 0:bd0546063b0a 464 }