Diff: main.cpp

Revision:

2:2fb847807890

Parent:

1:876f52a697c1

Child:

3:6dae4f871cdc

--- a/main.cpp	Sun Sep 22 17:44:42 2013 +0000
+++ b/main.cpp	Tue Aug 04 15:01:11 2015 +0000
@@ -3,25 +3,224 @@
 #include "mbed.h"
 #include "MMA7660.h"
 #include "C12832_lcd.h"
+#include "USBHostMSD.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
 
 C12832_LCD lcd; //On board LCD display
 MMA7660 MMA(p28, p27); //I2C Accelerometer
 DigitalOut connectionLed(LED1);//Accel OK LED
 
+#include "mbed.h"
+#include <string>
+
+//opens mbed memory for writing
+LocalFileSystem local("local");
+FILE *fp = fopen("/local/MovementData.txt", "w");
+
+//PwmOut strafe(p22);
+DigitalOut strafe(p21);
+DigitalOut forback(p22);
+DigitalOut updown(p23);
+DigitalOut rlturn(p24);
+AnalogOut output(p18);
+
+float StrafeVM = 0.48638;
+float ForBackVM = .508172;
+float UpDownVM = .50424;
+float RLTurnVM = .508475;
+
+float step = .001;
+
+void NoOut()
+{
+    strafe = 1;
+    forback = 1;
+    updown = 1;
+    rlturn = 1;
+}
+
+void SET(string movement, float Out)
+{
+    
+    NoOut();
+    
+    output = Out;
+
+    if (movement == "s") {
+        strafe = 0;
+    }
+
+    if (movement == "fb") {
+        forback = 0;
+    }
+
+    if (movement == "ud") {
+        updown = 0;
+    }
+
+    if (movement == "rl") {
+        rlturn = 0;
+    }
+
+    wait(.005);
+    
+     fprintf(fp,"%s ", (string)movement);
+     fprintf(fp,"%f ", (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.
+void REFRESH()
+{
+    SET("s", StrafeVM);
+    SET("fb", ForBackVM);
+    SET("ud", UpDownVM);
+    SET("rl", RLTurnVM);
+}
+
+void IDLE()
+{
+    SET("s", 0.48638);
+    SET("fb", 0.508172);
+    SET("ud", 0.50424);
+    SET("rl", 0.508475);
+}
+
+void START()
+{
+    SET("s", 0.575379);
+    SET("fb", 0.592171);
+    SET("ud", 0.421241);
+    SET("rl", 0.589474);
+    wait(3);
+}
+
 int main()
 {
-    int x=0,y=0;
-    lcd.cls(); //clear LCD screen
-    if (MMA.testConnection())
-        connectionLed = 1; //Accelerometer init OK
-    while(1) {
-        //read X,Y +/-Gs and scale for #display pixels
-        x = (x + MMA.x() * 32.0)/2.0;
-        y = (y -(MMA.y() * 16.0))/2.0;
-        lcd.fillcircle(x+63, y+15, 3, 1); //draw bubble
-        lcd.circle(63, 15, 8, 1);
-        wait(.1); //time delay
-        lcd.fillcircle(x+63, y+15, 3, 0); //erase bubble
+    bool on = false;
+    bool YN = true;
+
+    pc.printf("\n");
+    
+    lcd.cls();
+    lcd.locate(0,3);
+    lcd.printf("Turn Phantom 2 Drone On? (Y/N) ");
+    
+    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();
+            }
+            
+            //set c to be arbitrary
+            char c = '?';
+            
+            REFRESH();
+            
+            //pc.getc() hangs (stops) the loop until a caracter is read. due to the need to 
+            if (pc.readable()) {          
+                c = pc.getc();
+            }
+            
+            //strafing
+            if((c == 'a') && (StrafeVM < /*0.57234*/ .6)) {
+                StrafeVM += step;
+                SET("s", StrafeVM);
+                pc.printf("%f%", StrafeVM);
+            }
+            if((c == 'd') && (StrafeVM > 0.410714)) {
+                StrafeVM -= step;
+                SET("s", StrafeVM);
+                pc.printf("%f%", StrafeVM);
+            }
+
+            //Forward and Backwards
+            if((c == 'w') && (ForBackVM > 0.424031)) {
+                ForBackVM -= step;
+                SET("fb", ForBackVM);
+                pc.printf("%f%", ForBackVM);
+            }
+            if((c == 's') && (ForBackVM < /*0.58535*/ .6)) {
+                ForBackVM += step;
+                SET("fb", ForBackVM);
+                pc.printf("%f%", ForBackVM);
+            }
+
+            //Up and Down Issues with being off by .08v ish
+            if((c == 'i') && (UpDownVM < 0.58323)) {
+                UpDownVM += step;
+                SET("ud", UpDownVM);
+                pc.printf("%f%", UpDownVM);
+            }
+            if((c == 'k') && (UpDownVM > 0.42161)) {
+                UpDownVM -= step;
+                SET("ud", UpDownVM);
+                pc.printf("%f%", UpDownVM);
+            }
+
+            //Turning
+            if((c == 'j') && (RLTurnVM < 0.5905)) {
+                RLTurnVM += step;
+                SET("rl", RLTurnVM);
+                pc.printf("%f%", RLTurnVM);
+            }
+            if((c == 'l') && (RLTurnVM > 0.42615)) {
+                RLTurnVM -= step;
+                SET("rl", RLTurnVM);
+                pc.printf("%f%", RLTurnVM);
+            }
+
+            //emergency idle
+            if(c == ' ') {
+                IDLE();
+            }
+
+            //ground/turn off the drone
+            if(c == 'g') {
+                IDLE();
+
+                while(UpDownVM > 0.48) {
+                    UpDownVM -= .01;
+                    SET("ud", UpDownVM);
+                    wait(0.1);
+                }
+                wait(2);
+                
+                on = false;
+                
+            }
+
+        }
+
+    }
+    
+    fclose(fp);
+    pc.printf("Drone off");
+    while(1) {
+        SET("s", 0.48638);
+        SET("fb", 0.508172);
+        SET("ud", .421);
+        SET("rl", 0.508475);
+    }
+    return 0;
 }