![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
main.cpp
- Committer:
- ivo_david_michelle
- Date:
- 2016-05-07
- Revision:
- 51:b6d76a4dfae8
- Parent:
- 50:c9831bb5a660
File content as of revision 51:b6d76a4dfae8:
#include "mbed.h" #include "rtos.h" #define _MBED_ //#include "controller.h" #include "sensor.h" #include "quadcopter.h" Serial pc(USBTX, USBRX); MRF24J40 mrf(p11, p12, p13, p14, p21); Timer timer; // timer ;) //Mutex desired_mutex; int started = 0; 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); PwmOut motor_2(p24); PwmOut motor_3(p25); PwmOut motor_4(p26); // to read the battery voltage AnalogIn battery(p20); DigitalOut batteryLed(LED1); DigitalOut shutDownLed(LED2); //DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement int emergencyOff = 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; motor_1 = OFF_PWM; motor_2 = OFF_PWM; motor_3 = OFF_PWM; motor_4 = OFF_PWM; } 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); 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()); 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(); printf("too long negative thrust! %f\r\n",myQuadcopter.getForce()); shutDownLed = 1; break; } Thread::wait(10); Thread::yield(); } } */ bool low_battery() { float threshold_voltage = 13.0; // desired lowest battery voltage float emergencyVoltage = 12.5; // switch off motors below it float max_voltage = 14.8; // max voltage level of battery float saturating_voltage = 18.38; // voltage at which ADC == 1 float max_adc = 0.80522; // when battery is at 14.8V float threshold_adc = max_adc * threshold_voltage / max_voltage; float emergency_adc = max_adc * emergencyVoltage / max_voltage; 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; } } 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); // } //} int main() { // ESCs requires a 100Hz frequency motor_1.period(0.01); motor_2.period(0.01); motor_3.period(0.01); motor_4.period(0.01); //Thread counting1(counting_thread1); //Thread counting2(counting_thread2); //counting1.set_priority(osPriorityHigh); //counting2.set_priority(osPriorityHigh); //Thread thread_rc(rc_thread); printf("Waiting for start signal!\r\n"); 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. // start ESCs up with 10% duty cycle motor_1 = OFF_PWM; motor_2 = OFF_PWM; motor_3 = OFF_PWM; motor_4 = OFF_PWM; 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 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); int i = 0; 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; } } previousTime = currentTime; // can be used to measure frequency // TODO why did it get to 430Hz // if (i % 1000 == 0) { // currentTime = timer.read(); // printf("%d, %f, %f\r\n", i, 1000.0 / (currentTime - previousTime), currentTime); // previousTime = currentTime; // // } myQuadcopter.readRc(); // 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(); // 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; motor_4 = motorsPwm.m4; } }