Drone code for Prof. Coode
Dependencies: C12832_lcd FatFileSystemCpp MMA7660 mbed
Fork of app-board-Bubble-Level by
Revision 2:2fb847807890, committed 2015-08-04
- Comitter:
- ecleland
- Date:
- Tue Aug 04 15:01:11 2015 +0000
- Parent:
- 1:876f52a697c1
- Child:
- 3:6dae4f871cdc
- Commit message:
- Work please
Changed in this revision
USBHost.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBHost.lib Tue Aug 04 15:01:11 2015 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/USBHost/#37c948cf0dbf
--- 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; }