main.cpp

00001 
00002 #include "mbed.h"
00003 #include "MMA7660.h"
00004 #include "C12832_lcd.h"
00005 #include "MSCFileSystem.h"
00006 #include <string>
00007 
00008 
00009 Serial pc(USBTX, USBRX); // tx, rx
00010 
00011 C12832_LCD lcd; //On board LCD display
00012 MMA7660 MMA(p28, p27); //I2C Accelerometer
00013 DigitalOut connectionLed(LED1);//Accel OK LED
00014 
00015 MSCFileSystem fs("fs");
00016 FILE *fp = fopen("/fs/movement.txt","w");
00017 
00018 int numMovements = 0;
00019 bool rep = false;
00020 
00021 DigitalIn up(p15); 
00022 DigitalIn down(p12);
00023 DigitalIn left(p13);
00024 DigitalIn right(p16);
00025 DigitalIn pressed(p14);
00026 
00027 
00028 
00029 DigitalOut strafe(p21);
00030 DigitalOut forback(p22);
00031 DigitalOut updown(p23);
00032 DigitalOut rlturn(p24);
00033 AnalogOut output(p18);
00034 
00035 float StrafeVM = 0.48638;
00036 float ForBackVM = .508172;
00037 float UpDownVM = .50424;
00038 float RLTurnVM = .508475;
00039 
00040 float step = .001;
00041 
00042 
00043 
00044 void NoOut()
00045 {
00046     strafe = 1;
00047     forback = 1;
00048     updown = 1;
00049     rlturn = 1;
00050 }
00051 
00052 void SET(string movement, float Out)
00053 {    
00054    
00055     NoOut();
00056     
00057     output = Out;
00058 
00059     if (movement == "s") {
00060         strafe = 0;
00061     }
00062 
00063     if (movement == "fb") {
00064         forback = 0;
00065     }
00066 
00067     if (movement == "ud") {
00068         updown = 0;
00069     }
00070 
00071     if (movement == "rl") {
00072         rlturn = 0;
00073     }
00074     
00075     
00076     wait(.001);
00077     
00078     if (!rep){
00079      fprintf(fp,"%s %f ", (string)movement, (float)Out);
00080      numMovements ++;
00081      }
00082      //fprintf(fp,"%f\r\n", (float)Out);
00083      
00084     
00085     
00086     NoOut();
00087     
00088     
00089 }
00090 
00091 //latch chip drifts voltage up pretty quickly beyond the limits of the phantom if we reset the values though all is good.
00092 void REFRESH()
00093 {
00094     SET("s", StrafeVM);
00095     SET("fb", ForBackVM);
00096     SET("ud", UpDownVM);
00097     SET("rl", RLTurnVM);
00098 }
00099 
00100 void IDLE()
00101 {
00102     SET("s", 0.48638);
00103     SET("fb", 0.508172);
00104     SET("ud", 0.50424);
00105     SET("rl", 0.508475);
00106 }
00107 
00108 void START()
00109 {
00110     SET("s", 0.575379);
00111     SET("fb", 0.592171);
00112     SET("ud", 0.421241);
00113     SET("rl", 0.589474);
00114     wait(2);
00115 }
00116 
00117 void Repeat() {
00118     fclose(fp);
00119 
00120     rep = true;
00121     
00122     string movement;
00123     float Out;
00124     
00125     
00126     FILE *directions = fopen("/fs/movement.txt","r");
00127     if (directions == NULL) {
00128         lcd.printf("Cannot Open Movement Log For Repeat");
00129     }
00130     
00131     pc.printf("\r\n%d movements\r\n", numMovements);
00132     
00133     for (int i = 0; i < numMovements; i++) {
00134     fscanf(directions, "%s %f ", movement, &Out);
00135     
00136         
00137     SET(movement, Out);
00138     pc.printf("%d %s %f\r\n", i, movement, Out);
00139      
00140    
00141     }
00142     
00143     fclose(directions);
00144 }
00145     
00146 void repeatMenu()
00147 {
00148     wait(1);
00149     bool rep = true;
00150     
00151     lcd.cls();
00152     lcd.locate(0,0);
00153     lcd.printf("Retrace Flight Path?\nYES\nNO");
00154     lcd.locate(20,9);
00155     lcd.printf("<");
00156     
00157     while(1) {
00158         
00159         if(up) {
00160             rep = true;
00161             lcd.cls();
00162             lcd.locate(0,0);
00163             lcd.printf("Retrace Flight Path?\nYES\nNO");
00164             lcd.locate(20,9);
00165             lcd.printf("<");
00166             }
00167         
00168         if(down) {
00169             rep = false;
00170             lcd.cls();
00171             lcd.locate(0,0);
00172             lcd.printf("Retrace Flight Path?\nYES\nNO");
00173             lcd.locate(20,18);
00174             lcd.printf("<");
00175         }
00176         
00177         if(pressed) {
00178             lcd.cls();
00179             if(rep) {
00180                 lcd.locate(0,0);
00181                 lcd.printf("Place Drone at Starting\nPosition and Press Joystick\nto Begin.");
00182                 
00183                 while(!pressed){
00184                 }
00185                 lcd.locate(0,0);
00186                 lcd.cls();
00187                 lcd.printf("Beginning Retrace");
00188                 Repeat();
00189                 lcd.locate(0,0);
00190                 lcd.cls();
00191                 lcd.printf("Retrace Finished");
00192                 wait(2);  
00193                 return;
00194                 }            
00195             lcd.printf("Drone off.");
00196             return;
00197         }
00198                 
00199         
00200     }
00201 }
00202 
00203     
00204 
00205 int startupMenu() 
00206 {
00207    if (fp == NULL) {
00208         lcd.cls();
00209         lcd.locate(0,3);
00210         lcd.printf("Could Not Connect to USB FLash Drive.");
00211         wait(2);
00212     }
00213     
00214 
00215     bool on = true;
00216 
00217 
00218     
00219     lcd.cls();
00220     lcd.locate(0,0);
00221     lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
00222     lcd.locate(20,9);
00223     lcd.printf("<");
00224     
00225     while(1) {
00226         
00227         if(up) {
00228             on = true;
00229             lcd.cls();
00230             lcd.locate(0,0);
00231             lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
00232             lcd.locate(20,9);
00233             lcd.printf("<");
00234             }
00235         
00236         if(down) {
00237             on = false;
00238             lcd.cls();
00239             lcd.locate(0,0);
00240             lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
00241             lcd.locate(20,18);
00242             lcd.printf("<");
00243         }
00244         
00245         if(pressed) {
00246             lcd.cls();
00247             if(on) {
00248                 lcd.locate(0,0);
00249                 lcd.printf("Starting Drone...");
00250                 IDLE();
00251                 START();
00252                 IDLE();
00253                 lcd.cls();
00254                 lcd.locate(0,0);
00255                 lcd.printf("Drone On.");
00256                 return 1;
00257                 }            
00258             lcd.printf("Drone off.");
00259             return 0;
00260         }
00261                 
00262         
00263     }
00264 }
00265 
00266 int turnOffMenu() {   
00267     
00268     IDLE();
00269     wait(1);
00270     bool off = true;
00271     
00272     lcd.cls();
00273     lcd.locate(0,0);
00274     lcd.printf("Turn Phantom 2 Drone off?\nYES\nNO");
00275     lcd.locate(20,9);
00276     lcd.printf("<");
00277     
00278     while(1) {
00279         
00280         if(up) {
00281             off = true;
00282             lcd.cls();
00283             lcd.locate(0,0);
00284             lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
00285             lcd.locate(20,9);
00286             lcd.printf("<");
00287             }
00288         
00289         if(down) {
00290             off = false;
00291             lcd.cls();
00292             lcd.locate(0,0);
00293             lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
00294             lcd.locate(20,18);
00295             lcd.printf("<");
00296         }
00297         
00298         if(pressed) {
00299             lcd.cls();
00300             if(off) {
00301                 lcd.locate(0,0);
00302                 lcd.printf("Turning Drone Off...");
00303 
00304                 SET("s", 0.48638);
00305                 SET("fb", 0.508172);
00306                 SET("ud", .421);
00307                 SET("rl", 0.508475);
00308                 wait(3);
00309                 lcd.cls();
00310                 lcd.locate(0,0);
00311                 lcd.printf("Drone Off.");
00312                 return 0;
00313                 
00314                 } 
00315             lcd.locate(0,0);
00316             lcd.printf("Drone On.");   
00317             return 1;
00318         }              
00319     }
00320 }
00321 
00322 int main()
00323 {   
00324     int on = startupMenu();
00325     if(on) {
00326         
00327     
00328         
00329         
00330         
00331         //all limits need to be investigatged as vcc is not actually +3.304V also phantom 2 voltages vary based on battery charge in controller.
00332         while(on) {
00333             
00334             
00335             
00336             if(MMA.z() < 0) {
00337                 IDLE();    
00338                 START();
00339                 IDLE();
00340             }
00341             
00342             
00343             if(MMA.y() < -.6) {
00344                 ForBackVM -= step;
00345                 SET("fb", ForBackVM);
00346                 pc.printf("%f%", ForBackVM);
00347             }
00348             
00349             if(MMA.y() > .6) {
00350                 ForBackVM += step;
00351                 SET("fb", ForBackVM);
00352                 pc.printf("%f%", ForBackVM);
00353             }
00354             
00355             if(MMA.x() > .6) {
00356                 StrafeVM += step;
00357                 SET("s", StrafeVM);
00358                 pc.printf("%f%", StrafeVM);
00359             }
00360             
00361             if(MMA.x() < -.6) {
00362                 StrafeVM -= step;
00363                 SET("s", StrafeVM);
00364                 pc.printf("%f%", StrafeVM);
00365             }
00366             
00367             
00368             if(pressed) {
00369                 on = turnOffMenu();
00370                 if(!on) {
00371                     repeatMenu();
00372                 }
00373                 
00374                 }
00375             
00376             //set c to be arbitrary
00377             char c = '?';
00378             
00379             REFRESH();
00380             
00381             //pc.getc() hangs (stops) the loop until a caracter is read. due to the need to 
00382             if (pc.readable()) {          
00383                 c = pc.getc();
00384             }
00385             
00386             //strafing
00387             if((c == 'a') && (StrafeVM < /*0.57234*/ .6)) {
00388                 StrafeVM += step;
00389                 SET("s", StrafeVM);
00390                 pc.printf("%f%", StrafeVM);
00391             }
00392             if((c == 'd') && (StrafeVM > 0.410714)) {
00393                 StrafeVM -= step;
00394                 SET("s", StrafeVM);
00395                 pc.printf("%f%", StrafeVM);
00396             }
00397 
00398             //Forward and Backwards
00399             if((c == 'w') && (ForBackVM > 0.424031)) {
00400                 ForBackVM -= step;
00401                 SET("fb", ForBackVM);
00402                 pc.printf("%f%", ForBackVM);
00403             }
00404             if((c == 's') && (ForBackVM < /*0.58535*/ .6)) {
00405                 ForBackVM += step;
00406                 SET("fb", ForBackVM);
00407                 pc.printf("%f%", ForBackVM);
00408             }
00409 
00410             //Up and Down Issues with being off by .08v ish
00411             if((c == 'i') && (UpDownVM < 0.58323)) {
00412                 UpDownVM += step;
00413                 SET("ud", UpDownVM);
00414                 pc.printf("%f%", UpDownVM);
00415             }
00416             if((c == 'k') && (UpDownVM > 0.42161)) {
00417                 UpDownVM -= step;
00418                 SET("ud", UpDownVM);
00419                 pc.printf("%f%", UpDownVM);
00420             }
00421 
00422             //Turning
00423             if((c == 'j') && (RLTurnVM < 0.5905)) {
00424                 RLTurnVM += step;
00425                 SET("rl", RLTurnVM);
00426                 pc.printf("%f%", RLTurnVM);
00427             }
00428             if((c == 'l') && (RLTurnVM > 0.42615)) {
00429                 RLTurnVM -= step;
00430                 SET("rl", RLTurnVM);
00431                 pc.printf("%f%", RLTurnVM);
00432             }
00433 
00434             //emergency idle
00435             if(c == ' ') {
00436                 IDLE();
00437             }
00438 
00439             //ground/turn off the drone
00440             if(c == 'g') {
00441                 IDLE();
00442 
00443                 while(UpDownVM > 0.48) {
00444                     UpDownVM -= .01;
00445                     SET("ud", UpDownVM);
00446                     wait(0.1);
00447                 }
00448                 wait(2);
00449                 
00450                 on = 0;
00451                 
00452             }
00453 
00454         }
00455        fclose(fp); 
00456 
00457     }
00458     
00459     
00460     lcd.cls();
00461     lcd.locate(0,0);
00462     lcd.printf("Program Finished");
00463     return 0;
00464 }