![](/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-04
- Revision:
- 42:d09dec5bb184
- Parent:
- 40:09a59d5b7944
- Child:
- 43:0e98d5488bf2
File content as of revision 42:d09dec5bb184:
#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, &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 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 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; 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. 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 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); //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; 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); previousTime = currentTime; } */ 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(); // 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(); motors motorsPwm = myQuadcopter.getPwm(); motor_1 = motorsPwm.m1; motor_2 = motorsPwm.m2; motor_3 = motorsPwm.m3; motor_4 = motorsPwm.m4; } }