File content as of revision 3:6dae4f871cdc:

#include "mbed.h"
#include "MMA7660.h"
#include "C12832_lcd.h"
#include "MSCFileSystem.h"
#include <string>


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

MSCFileSystem fs("fs");
FILE *fp = fopen("/fs/movement.txt","w");

int numMovements = 0;
bool rep = false;

DigitalIn up(p15); 
DigitalIn down(p12);
DigitalIn left(p13);
DigitalIn right(p16);
DigitalIn pressed(p14);



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(.001);
    
    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.
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(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()
{   
    int on = startupMenu();
    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 = '?';
            
            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 = 0;
                
            }

        }
       fclose(fp); 

    }
    
    
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("Program Finished");
    return 0;
}