File content as of revision 2:2fb847807890:

//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"

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()
{
    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;
}