ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: main.cpp
- Revision:
- 38:14bf11115f9f
- Parent:
- 37:a983eb9fd9c5
- Child:
- 39:fff0a72633ee
diff -r a983eb9fd9c5 -r 14bf11115f9f main.cpp --- a/main.cpp Sun May 01 18:45:02 2016 +0000 +++ b/main.cpp Sun May 01 21:59:10 2016 +0000 @@ -24,17 +24,19 @@ AnalogIn battery(p20); DigitalOut batteryLed(LED1); DigitalOut shutDownLed(LED2); +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) { +void print_count(int id) +{ static int count = 0; if (count % 1000 == 0) { printf("%d\t%d\r\n", id, count); } - count++; + count++; } void emergencyShutdown() @@ -65,7 +67,7 @@ void controller_thread(void const *args) { while(emergencyOff != 1) { - // printf(" thrust: %f\r\n",myQuadcopter.getForce()); + // printf(" thrust: %f\r\n",myQuadcopter.getForce()); myQuadcopter.readSensorValues(); myQuadcopter.controller(); @@ -94,11 +96,11 @@ if (count % 1000 == 0) { printf("%s\t%d\r\n", "rc", count); } - count++; + count++; myQuadcopter.readRc(); if (!started) { - continue; + continue; } int shutdown = getLowThrust(-0.3); if (shutdown==1) { @@ -127,7 +129,7 @@ if (count % 1 == 0) { //printf("%s\t%d\r\n", "battery", count); } - count++; + count++; if (battery.read() < threshold_adc) { printf("low battery! %f\r\n", battery.read() * saturating_voltage); batteryLed = 1; @@ -140,14 +142,16 @@ } } -void counting_thread1(void const *args) { +void counting_thread1(void const *args) +{ while (true) { print_count(1); Thread::wait(1); } } -void counting_thread2(void const *args) { +void counting_thread2(void const *args) +{ while (true) { print_count(2); } @@ -185,16 +189,27 @@ //Thread battery(battery_thread); //Thread controller(controller_thread); // TODO is this needed? - + //controller.set_priority(osPriorityRealtime); //thread_rc.set_priority(osPriorityRealtime); - int i =0; + int i = 0; + float previousTime =0; + float currentTime = 0; while (1) { i++; - if (i % 100 == 0) { - printf("%d\r\n", i); - } + + /* if (i % 5000 == 0) { + currentTime = timer.read(); + printf("%d, %f, %f\r\n", i, 5000.0 / (currentTime - previousTime), currentTime); + previousTime = currentTime; + + } */ + + myQuadcopter.readRc(); + + + int shutdown = getLowThrust(-0.3); if (shutdown == 1) { emergencyShutdown(); @@ -203,6 +218,14 @@ break; } myQuadcopter.readSensorValues(); + /* state mystate = myQuadcopter.getState(); + if (mystate.phi > 0.0785) { + quicknessLed = 1; + } else { + quicknessLed = 0; + } */ + + myQuadcopter.controller(); motors motorsPwm = myQuadcopter.getPwm(); motor_1 = motorsPwm.m1;