ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 30:4820042e67b5
- Parent:
- 29:ae765492fa8b
- Child:
- 31:d473eacfc271
diff -r ae765492fa8b -r 4820042e67b5 main.cpp --- a/main.cpp Fri Apr 15 19:33:14 2016 +0000 +++ b/main.cpp Wed Apr 20 02:29:37 2016 +0000 @@ -18,12 +18,49 @@ // to read the battery voltage AnalogIn battery(p20); DigitalOut batteryLed(LED1); +DigitalOut shutDownLed(LED2); int emergencyOff = 0; +//int lowThrust= {0,0,0}; +int nLowThrust = 0; + +void emergencyShutdown() +{ + emergencyOff = 1; + motor_1 = 0.1; + motor_2 = 0.1; + motor_3 = 0.1; + motor_4 = 0.1; +} + +int getLowThrust(double threshold) +{ + double force = myQuadcopter.getForce(); + if (force < threshold) { // if low thrust signal is detected + nLowThrust++; + printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust); + if (nLowThrust > 5) { + return 1; + } + } else { + nLowThrust = 0; + } + return 0; +} void controller_thread(void const *args) { while(emergencyOff != 1) { + // printf(" thrust: %f\r\n",myQuadcopter.getForce()); + + int shutdown= getLowThrust(-0.3); + if (shutdown==1) { + emergencyShutdown(); + printf("too long negative thrust! %f\r\n",myQuadcopter.getForce()); + shutDownLed = 1; + break; + } + myQuadcopter.readSensorValues(); myQuadcopter.controller(); @@ -60,11 +97,8 @@ printf("low battery! %f\r\n", battery.read() * saturating_voltage); batteryLed = 1; if (battery.read() < emergency_adc) { - emergencyOff = 1; - motor_1 = 0.1; - motor_2 = 0.1; - motor_3 = 0.1; - motor_4 = 0.1; + emergencyShutdown(); + break; } } Thread::wait(1000); // wait for some number of miliseconds @@ -82,21 +116,21 @@ motor_4.period(0.01); Thread threadR(rc_thread); - double forceThreshold = -0.3; - double forceRc = myQuadcopter.getForce(); + + + int startLoop= 0; - // TODO figure out why forceRc occasionally goes below the threshold without input - while (forceRc > forceThreshold) { // wait until Joystick is in starting position - forceRc = myQuadcopter.getForce(); - wait(1); + while (!startLoop) { // wait until Joystick is in starting position + startLoop= getLowThrust(-0.3); } + nLowThrust = 0; // reset for later use in controller. motor_1 = 0.1; motor_2 = 0.1; motor_3 = 0.1; motor_4 = 0.1; - wait(2); // hold startup duty cycle for 2 seconds + wait(3); // hold startup duty cycle for 2 seconds // TODO assign priorities to threads, test if it really works as we expect Thread battery(battery_thread);