ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 37:a983eb9fd9c5
- Parent:
- 36:40b134328376
- Child:
- 38:14bf11115f9f
diff -r 40b134328376 -r a983eb9fd9c5 main.cpp --- a/main.cpp Fri Apr 29 18:24:15 2016 +0000 +++ b/main.cpp Sun May 01 18:45:02 2016 +0000 @@ -9,6 +9,9 @@ MRF24J40 mrf(p11, p12, p13, p14, p21); Timer timer; // timer ;) Mutex desired_mutex; + +int started = 0; + Quadcopter myQuadcopter(&pc, &mrf, &timer, &desired_mutex); // instantiate Quadcopter object // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction @@ -26,6 +29,14 @@ //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 emergencyShutdown() { emergencyOff = 1; @@ -38,6 +49,7 @@ int getLowThrust(double threshold) { double force = myQuadcopter.getForce(); + //printf("%f\r\n", force); if (force < threshold) { // if low thrust signal is detected nLowThrust++; printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust); @@ -54,7 +66,40 @@ { while(emergencyOff != 1) { // printf(" thrust: %f\r\n",myQuadcopter.getForce()); + myQuadcopter.readSensorValues(); + myQuadcopter.controller(); + static int count = 0; + if (count % 100 == 0) { + printf("%s\t%d\r\n", "controller", count); + } + count++; + motors motorsPwm = myQuadcopter.getPwm(); + + motor_1 = motorsPwm.m1; + motor_2 = motorsPwm.m2; + motor_3 = motorsPwm.m3; + motor_4 = motorsPwm.m4; + + Thread::wait(10); + Thread::yield(); + // 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; + while(true) { + if (count % 1000 == 0) { + printf("%s\t%d\r\n", "rc", count); + } + count++; + myQuadcopter.readRc(); + + if (!started) { + continue; + } int shutdown = getLowThrust(-0.3); if (shutdown==1) { emergencyShutdown(); @@ -62,25 +107,8 @@ shutDownLed = 1; break; } - - myQuadcopter.readSensorValues(); - - myQuadcopter.controller(); - motors motorsPwm = myQuadcopter.getPwm(); - - motor_1 = motorsPwm.m1; - motor_2 = motorsPwm.m2; - motor_3 = motorsPwm.m3; - motor_4 = motorsPwm.m4; - - // 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) -{ - while(true) { - myQuadcopter.readRc(); + Thread::wait(10); + Thread::yield(); } } @@ -94,7 +122,12 @@ 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); + } + count++; if (battery.read() < threshold_adc) { printf("low battery! %f\r\n", battery.read() * saturating_voltage); batteryLed = 1; @@ -107,6 +140,19 @@ } } +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() { // ESCs requires a 100Hz frequency @@ -115,13 +161,17 @@ motor_3.period(0.01); motor_4.period(0.01); - Thread thread_rc(rc_thread); - - int startLoop = 0; + //Thread counting1(counting_thread1); + //Thread counting2(counting_thread2); + //counting1.set_priority(osPriorityHigh); + //counting2.set_priority(osPriorityHigh); + //Thread thread_rc(rc_thread); - while (!startLoop) { // wait until Joystick is in starting position - startLoop = getLowThrust(-0.3); + while (!started) { // wait until Joystick is in starting position + myQuadcopter.readRc(); + started = getLowThrust(-0.3); } + printf("Started!\r\n"); nLowThrust = 0; // reset for later use in controller. motor_1 = 0.1; @@ -132,8 +182,32 @@ 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); - Thread controller(controller_thread); + //Thread battery(battery_thread); + //Thread controller(controller_thread); // TODO is this needed? - while (1); + + //controller.set_priority(osPriorityRealtime); + //thread_rc.set_priority(osPriorityRealtime); + int i =0; + while (1) { + i++; + if (i % 100 == 0) { + printf("%d\r\n", i); + } + myQuadcopter.readRc(); + 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(); + motors motorsPwm = myQuadcopter.getPwm(); + motor_1 = motorsPwm.m1; + motor_2 = motorsPwm.m2; + motor_3 = motorsPwm.m3; + motor_4 = motorsPwm.m4; + } }