ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 42:d09dec5bb184
- Parent:
- 40:09a59d5b7944
- Child:
- 43:0e98d5488bf2
diff -r d103f9aa44f0 -r d09dec5bb184 main.cpp --- a/main.cpp Wed May 04 18:46:06 2016 +0000 +++ b/main.cpp Wed May 04 21:35:05 2016 +0000 @@ -42,10 +42,10 @@ void emergencyShutdown() { emergencyOff = 1; - motor_1 = 0.1; - motor_2 = 0.1; - motor_3 = 0.1; - motor_4 = 0.1; + motor_1 = OFF_PWM; + motor_2 = OFF_PWM; + motor_3 = OFF_PWM; + motor_4 = OFF_PWM; } int getLowThrust(double threshold) @@ -114,7 +114,7 @@ } } -void battery_thread(void const *args) +bool low_battery() { float threshold_voltage = 13.0; // desired lowest battery voltage float emergencyVoltage = 12.5; // switch off motors below it @@ -124,22 +124,15 @@ float threshold_adc = max_adc * threshold_voltage / max_voltage; float emergency_adc = max_adc * emergencyVoltage / max_voltage; - static int count = 0; - while(true) { - if (count % 1 == 0) { - //printf("%s\t%d\r\n", "battery", count); + if (battery.read() < threshold_adc) { + printf("low battery! %f\r\n", battery.read() * saturating_voltage); + batteryLed = 1; + if (battery.read() < emergency_adc) { + emergencyShutdown(); + return true; } - count++; - if (battery.read() < threshold_adc) { - printf("low battery! %f\r\n", battery.read() * saturating_voltage); - batteryLed = 1; - if (battery.read() < emergency_adc) { - emergencyShutdown(); - break; - } - } - Thread::wait(1000); // wait for some number of miliseconds } + return false; } void counting_thread1(void const *args) @@ -180,12 +173,19 @@ printf("Started!\r\n"); nLowThrust = 0; // reset for later use in controller. - motor_1 = 0.1; - motor_2 = 0.1; - motor_3 = 0.1; - motor_4 = 0.1; + motor_1 = OFF_PWM; // starts at 11.72% duty cycle + motor_2 = OFF_PWM; // starts at 11.61% duty cycle + motor_3 = OFF_PWM; // starts at 11.61% duty cycle + motor_4 = OFF_PWM; // starts at 11.71% duty cycle - wait(3); // hold startup duty cycle for 2 seconds + wait(1); // hold startup duty cycle for 2 seconds + + motor_1 = MIN_PWM_1; // starts at 11.72% duty cycle + motor_2 = MIN_PWM_2; // starts at 11.61% duty cycle + motor_3 = MIN_PWM_3; // starts at 11.61% duty cycle + motor_4 = MIN_PWM_4; // starts at 11.71% duty cycle + + wait(1); // hold startup duty cycle for 2 seconds // TODO assign priorities to threads, test if it really works as we expect //Thread battery(battery_thread); @@ -200,9 +200,14 @@ float currentTime = 0; while (1) { i++; + if (i % 1000 == 0) { + if (low_battery()) { + break; + } + } // can be used to measure frequency - /* + /* if (i % 5000 == 0) { currentTime = timer.read(); printf("%d, %f, %f\r\n", i, 5000.0 / (currentTime - previousTime), currentTime); @@ -210,9 +215,9 @@ } */ - + myQuadcopter.readRc(); - + int shutdown = getLowThrust(-0.3); if (shutdown == 1) { emergencyShutdown(); @@ -221,15 +226,15 @@ break; } myQuadcopter.readSensorValues(); - + // can be used to test responsiveness of the IMU - /* + /* state mystate = myQuadcopter.getState(); if (mystate.phi > 0.0785) { quicknessLed = 1; } else { quicknessLed = 0; - } + } */ myQuadcopter.controller();