Diff: main.cpp

Revision:

3:6dae4f871cdc

Parent:

2:2fb847807890

--- a/main.cpp	Tue Aug 04 15:01:11 2015 +0000
+++ b/main.cpp	Thu Sep 10 18:22:29 2015 +0000
@@ -1,9 +1,10 @@
-//Uses x & y acceleration to simulate a bubble level
-//on the application board LCD display
+
 #include "mbed.h"
 #include "MMA7660.h"
 #include "C12832_lcd.h"
-#include "USBHostMSD.h"
+#include "MSCFileSystem.h"
+#include <string>
+
 
 Serial pc(USBTX, USBRX); // tx, rx
 
@@ -11,14 +12,20 @@
 MMA7660 MMA(p28, p27); //I2C Accelerometer
 DigitalOut connectionLed(LED1);//Accel OK LED
 
-#include "mbed.h"
-#include <string>
+MSCFileSystem fs("fs");
+FILE *fp = fopen("/fs/movement.txt","w");
+
+int numMovements = 0;
+bool rep = false;
 
-//opens mbed memory for writing
-LocalFileSystem local("local");
-FILE *fp = fopen("/local/MovementData.txt", "w");
+DigitalIn up(p15); 
+DigitalIn down(p12);
+DigitalIn left(p13);
+DigitalIn right(p16);
+DigitalIn pressed(p14);
 
-//PwmOut strafe(p22);
+
+
 DigitalOut strafe(p21);
 DigitalOut forback(p22);
 DigitalOut updown(p23);
@@ -32,6 +39,8 @@
 
 float step = .001;
 
+
+
 void NoOut()
 {
     strafe = 1;
@@ -41,8 +50,8 @@
 }
 
 void SET(string movement, float Out)
-{
-    
+{    
+   
     NoOut();
     
     output = Out;
@@ -62,14 +71,21 @@
     if (movement == "rl") {
         rlturn = 0;
     }
-
-    wait(.005);
+    
+    
+    wait(.001);
     
-     fprintf(fp,"%s ", (string)movement);
-     fprintf(fp,"%f ", (float)Out);
+    if (!rep){
+     fprintf(fp,"%s %f ", (string)movement, (float)Out);
+     numMovements ++;
+     }
+     //fprintf(fp,"%f\r\n", (float)Out);
+     
+    
     
     NoOut();
-
+    
+    
 }
 
 //latch chip drifts voltage up pretty quickly beyond the limits of the phantom if we reset the values though all is good.
@@ -95,43 +111,268 @@
     SET("fb", 0.592171);
     SET("ud", 0.421241);
     SET("rl", 0.589474);
-    wait(3);
+    wait(2);
+}
+
+void Repeat() {
+    fclose(fp);
+
+    rep = true;
+    
+    string movement;
+    float Out;
+    
+    
+    FILE *directions = fopen("/fs/movement.txt","r");
+    if (directions == NULL) {
+        lcd.printf("Cannot Open Movement Log For Repeat");
+    }
+    
+    pc.printf("\r\n%d movements\r\n", numMovements);
+    
+    for (int i = 0; i < numMovements; i++) {
+    fscanf(directions, "%s %f ", movement, &Out);
+    
+        
+    SET(movement, Out);
+    pc.printf("%d %s %f\r\n", i, movement, Out);
+     
+   
+    }
+    
+    fclose(directions);
+}
+    
+void repeatMenu()
+{
+    wait(1);
+    bool rep = true;
+    
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Retrace Flight Path?\nYES\nNO");
+    lcd.locate(20,9);
+    lcd.printf("<");
+    
+    while(1) {
+        
+        if(up) {
+            rep = true;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Retrace Flight Path?\nYES\nNO");
+            lcd.locate(20,9);
+            lcd.printf("<");
+            }
+        
+        if(down) {
+            rep = false;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Retrace Flight Path?\nYES\nNO");
+            lcd.locate(20,18);
+            lcd.printf("<");
+        }
+        
+        if(pressed) {
+            lcd.cls();
+            if(rep) {
+                lcd.locate(0,0);
+                lcd.printf("Place Drone at Starting\nPosition and Press Joystick\nto Begin.");
+                
+                while(!pressed){
+                }
+                lcd.locate(0,0);
+                lcd.cls();
+                lcd.printf("Beginning Retrace");
+                Repeat();
+                lcd.locate(0,0);
+                lcd.cls();
+                lcd.printf("Retrace Finished");
+                wait(2);  
+                return;
+                }            
+            lcd.printf("Drone off.");
+            return;
+        }
+                
+        
+    }
+}
+
+    
+
+int startupMenu() 
+{
+   if (fp == NULL) {
+        lcd.cls();
+        lcd.locate(0,3);
+        lcd.printf("Could Not Connect to USB FLash Drive.");
+        wait(2);
+    }
+    
+
+    bool on = true;
+
+
+    
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
+    lcd.locate(20,9);
+    lcd.printf("<");
+    
+    while(1) {
+        
+        if(up) {
+            on = true;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
+            lcd.locate(20,9);
+            lcd.printf("<");
+            }
+        
+        if(down) {
+            on = false;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
+            lcd.locate(20,18);
+            lcd.printf("<");
+        }
+        
+        if(pressed) {
+            lcd.cls();
+            if(on) {
+                lcd.locate(0,0);
+                lcd.printf("Starting Drone...");
+                IDLE();
+                START();
+                IDLE();
+                lcd.cls();
+                lcd.locate(0,0);
+                lcd.printf("Drone On.");
+                return 1;
+                }            
+            lcd.printf("Drone off.");
+            return 0;
+        }
+                
+        
+    }
+}
+
+int turnOffMenu() {   
+    
+    IDLE();
+    wait(1);
+    bool off = true;
+    
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Turn Phantom 2 Drone off?\nYES\nNO");
+    lcd.locate(20,9);
+    lcd.printf("<");
+    
+    while(1) {
+        
+        if(up) {
+            off = true;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
+            lcd.locate(20,9);
+            lcd.printf("<");
+            }
+        
+        if(down) {
+            off = false;
+            lcd.cls();
+            lcd.locate(0,0);
+            lcd.printf("Turn Phantom 2 Drone On?\nYES\nNO");
+            lcd.locate(20,18);
+            lcd.printf("<");
+        }
+        
+        if(pressed) {
+            lcd.cls();
+            if(off) {
+                lcd.locate(0,0);
+                lcd.printf("Turning Drone Off...");
+
+                SET("s", 0.48638);
+                SET("fb", 0.508172);
+                SET("ud", .421);
+                SET("rl", 0.508475);
+                wait(3);
+                lcd.cls();
+                lcd.locate(0,0);
+                lcd.printf("Drone Off.");
+                return 0;
+                
+                } 
+            lcd.locate(0,0);
+            lcd.printf("Drone On.");   
+            return 1;
+        }              
+    }
 }
 
 int main()
-{
-    bool on = false;
-    bool YN = true;
-
-    pc.printf("\n");
-    
-    lcd.cls();
-    lcd.locate(0,3);
-    lcd.printf("Turn Phantom 2 Drone On? (Y/N) ");
+{   
+    int on = startupMenu();
+    if(on) {
+        
     
-    while(YN) {
-        char a = pc.getc();
-        if (a == 'y' || a == 'Y') {
-            on = true;
-            YN = false;
-        }
-        if (a == 'n' || a == 'N') {
-            YN = false;
-        }
-
-    }
-
-    if(on) {      
+        
+        
         
         //all limits need to be investigatged as vcc is not actually +3.304V also phantom 2 voltages vary based on battery charge in controller.
         while(on) {
             
+            
+            
             if(MMA.z() < 0) {
                 IDLE();    
                 START();
                 IDLE();
             }
             
+            
+            if(MMA.y() < -.6) {
+                ForBackVM -= step;
+                SET("fb", ForBackVM);
+                pc.printf("%f%", ForBackVM);
+            }
+            
+            if(MMA.y() > .6) {
+                ForBackVM += step;
+                SET("fb", ForBackVM);
+                pc.printf("%f%", ForBackVM);
+            }
+            
+            if(MMA.x() > .6) {
+                StrafeVM += step;
+                SET("s", StrafeVM);
+                pc.printf("%f%", StrafeVM);
+            }
+            
+            if(MMA.x() < -.6) {
+                StrafeVM -= step;
+                SET("s", StrafeVM);
+                pc.printf("%f%", StrafeVM);
+            }
+            
+            
+            if(pressed) {
+                on = turnOffMenu();
+                if(!on) {
+                    repeatMenu();
+                }
+                
+                }
+            
             //set c to be arbitrary
             char c = '?';
             
@@ -206,21 +447,18 @@
                 }
                 wait(2);
                 
-                on = false;
+                on = 0;
                 
             }
 
         }
+       fclose(fp); 
 
     }
     
-    fclose(fp);
-    pc.printf("Drone off");
-    while(1) {
-        SET("s", 0.48638);
-        SET("fb", 0.508172);
-        SET("ud", .421);
-        SET("rl", 0.508475);
-    }
+    
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Program Finished");
     return 0;
 }