ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 46:4bcf2e679f96
- Parent:
- 45:9f74298eee78
- Child:
- 47:ae478c380136
diff -r 9f74298eee78 -r 4bcf2e679f96 main.cpp --- a/main.cpp Thu May 05 14:42:30 2016 +0000 +++ b/main.cpp Thu May 05 15:58:49 2016 +0000 @@ -8,11 +8,11 @@ Serial pc(USBTX, USBRX); MRF24J40 mrf(p11, p12, p13, p14, p21); Timer timer; // timer ;) -Mutex desired_mutex; +//Mutex desired_mutex; int started = 0; -Quadcopter myQuadcopter(&pc, &mrf, &timer, &desired_mutex); // instantiate Quadcopter object +Quadcopter myQuadcopter(&pc, &mrf, &timer); // instantiate Quadcopter object // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction PwmOut motor_1(p23); @@ -24,20 +24,19 @@ AnalogIn battery(p20); DigitalOut batteryLed(LED1); DigitalOut shutDownLed(LED2); -DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement +//DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement int emergencyOff = 0; -//int lowThrust= {0,0,0}; int nLowThrust = 0; -void print_count(int id) -{ - static int count = 0; - if (count % 1000 == 0) { - printf("%d\t%d\r\n", id, count); - } - count++; -} +//void print_count(int id) +//{ +// static int count = 0; +// if (count % 1000 == 0) { +// printf("%d\t%d\r\n", id, count); +// } +// count++; +//} void emergencyShutdown() { @@ -64,7 +63,7 @@ return 0; } -void controller_thread(void const *args) +/*void controller_thread(void const *args) { while(emergencyOff != 1) { // printf(" thrust: %f\r\n",myQuadcopter.getForce()); @@ -88,7 +87,9 @@ // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r", motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4); } } +*/ +/* void rc_thread(void const *args) { static int count = 0; @@ -113,7 +114,7 @@ Thread::yield(); } } - +*/ bool low_battery() { float threshold_voltage = 13.0; // desired lowest battery voltage @@ -135,20 +136,20 @@ return false; } -void counting_thread1(void const *args) -{ - while (true) { - print_count(1); - Thread::wait(1); - } -} - -void counting_thread2(void const *args) -{ - while (true) { - print_count(2); - } -} +//void counting_thread1(void const *args) +//{ +// while (true) { +// print_count(1); +// Thread::wait(1); +// } +//} +// +//void counting_thread2(void const *args) +//{ +// while (true) { +// print_count(2); +// } +//} int main() { @@ -164,7 +165,7 @@ //counting2.set_priority(osPriorityHigh); //Thread thread_rc(rc_thread); - printf("waiting for start signal!\r\n"); + printf("Waiting for start signal!\r\n"); while (!started) { // wait until Joystick is in starting position myQuadcopter.readRc(); @@ -172,26 +173,26 @@ } printf("Started!\r\n"); nLowThrust = 0; // reset for later use in controller. + // start ESCs up with 10% duty cycle + motor_1 = OFF_PWM; + motor_2 = OFF_PWM; + motor_3 = OFF_PWM; + motor_4 = OFF_PWM; - 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(1); // hold startup duty cycle for 2 seconds + wait(1); // hold startup duty cycle for 1 second + // Idle thrust 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 + wait(1); // hold idle thrust for 1 second // TODO assign priorities to threads, test if it really works as we expect //Thread battery(battery_thread); //Thread controller(controller_thread); // TODO is this needed? - //controller.set_priority(osPriorityRealtime); //thread_rc.set_priority(osPriorityRealtime); @@ -199,15 +200,19 @@ float previousTime = 0; float currentTime = 0; float checkBattery = 0.0; + int shutdown = 0; + + // Enter while loop with control while (1) { + // Check battery level i++; currentTime = timer.read(); checkBattery += currentTime - previousTime; if (checkBattery > 1.0) { checkBattery = 0.0; - if (low_battery()) { - break; - } + // if (low_battery()) { +// break; +// } } previousTime = currentTime; @@ -220,30 +225,22 @@ // // } - myQuadcopter.readRc(); - int shutdown = getLowThrust(-0.3); + // shutdown + shutdown = getLowThrust(-0.3); if (shutdown == 1) { emergencyShutdown(); printf("too long negative thrust! %f\r\n",myQuadcopter.getForce()); shutDownLed = 1; break; } + // read sensor values 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; - } - */ - + // compute controller and get desired pwm myQuadcopter.controller(); motors motorsPwm = myQuadcopter.getPwm(); + // set pwm motor_1 = motorsPwm.m1; motor_2 = motorsPwm.m2; motor_3 = motorsPwm.m3;