David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@31:d473eacfc271, 2016-04-21 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Apr 21 20:57:19 2016 +0000
Revision:
31:d473eacfc271
Parent:
30:4820042e67b5
Child:
34:eaea0ae92dfa
implemented integral control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 0:4c04e4fd1310 1 #include "mbed.h"
ivo_david_michelle 14:64b06476d943 2 #include "rtos.h"
ivo_david_michelle 1:b87e95907a18 3 #define _MBED_
ivo_david_michelle 13:291ba30c7806 4 //#include "controller.h"
ivo_david_michelle 3:828e82089564 5 #include "sensor.h"
ivo_david_michelle 7:f3f94eadc5b5 6 #include "quadcopter.h"
ivo_david_michelle 7:f3f94eadc5b5 7
ivo_david_michelle 0:4c04e4fd1310 8 Serial pc(USBTX, USBRX);
ivo_david_michelle 15:90e07946186f 9 MRF24J40 mrf(p11, p12, p13, p14, p21);
ivo_david_michelle 15:90e07946186f 10 Quadcopter myQuadcopter(&pc, &mrf); // instantiate Quadcopter object
ivo_david_michelle 7:f3f94eadc5b5 11
ivo_david_michelle 6:6f3ffd97d808 12 // pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
ivo_david_michelle 15:90e07946186f 13 PwmOut motor_1(p23);
ivo_david_michelle 15:90e07946186f 14 PwmOut motor_2(p24);
ivo_david_michelle 15:90e07946186f 15 PwmOut motor_3(p25);
ivo_david_michelle 15:90e07946186f 16 PwmOut motor_4(p26);
ivo_david_michelle 15:90e07946186f 17
ivo_david_michelle 22:92401a4fec13 18 // to read the battery voltage
ivo_david_michelle 22:92401a4fec13 19 AnalogIn battery(p20);
ivo_david_michelle 26:7f50323c0c0d 20 DigitalOut batteryLed(LED1);
ivo_david_michelle 30:4820042e67b5 21 DigitalOut shutDownLed(LED2);
ivo_david_michelle 26:7f50323c0c0d 22
ivo_david_michelle 31:d473eacfc271 23 Timer timer; // timer ;)
ivo_david_michelle 31:d473eacfc271 24
ivo_david_michelle 28:61f7356325c3 25 int emergencyOff = 0;
ivo_david_michelle 30:4820042e67b5 26 //int lowThrust= {0,0,0};
ivo_david_michelle 30:4820042e67b5 27 int nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 28
ivo_david_michelle 30:4820042e67b5 29 void emergencyShutdown()
ivo_david_michelle 30:4820042e67b5 30 {
ivo_david_michelle 30:4820042e67b5 31 emergencyOff = 1;
ivo_david_michelle 30:4820042e67b5 32 motor_1 = 0.1;
ivo_david_michelle 30:4820042e67b5 33 motor_2 = 0.1;
ivo_david_michelle 30:4820042e67b5 34 motor_3 = 0.1;
ivo_david_michelle 30:4820042e67b5 35 motor_4 = 0.1;
ivo_david_michelle 30:4820042e67b5 36 }
ivo_david_michelle 30:4820042e67b5 37
ivo_david_michelle 30:4820042e67b5 38 int getLowThrust(double threshold)
ivo_david_michelle 30:4820042e67b5 39 {
ivo_david_michelle 30:4820042e67b5 40 double force = myQuadcopter.getForce();
ivo_david_michelle 30:4820042e67b5 41 if (force < threshold) { // if low thrust signal is detected
ivo_david_michelle 30:4820042e67b5 42 nLowThrust++;
ivo_david_michelle 30:4820042e67b5 43 printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
ivo_david_michelle 30:4820042e67b5 44 if (nLowThrust > 5) {
ivo_david_michelle 30:4820042e67b5 45 return 1;
ivo_david_michelle 30:4820042e67b5 46 }
ivo_david_michelle 30:4820042e67b5 47 } else {
ivo_david_michelle 30:4820042e67b5 48 nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 49 }
ivo_david_michelle 30:4820042e67b5 50 return 0;
ivo_david_michelle 30:4820042e67b5 51 }
ivo_david_michelle 4:3040d0f9e8c6 52
ivo_david_michelle 15:90e07946186f 53 void controller_thread(void const *args)
ivo_david_michelle 15:90e07946186f 54 {
ivo_david_michelle 26:7f50323c0c0d 55 while(emergencyOff != 1) {
ivo_david_michelle 30:4820042e67b5 56 // printf(" thrust: %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 30:4820042e67b5 57
ivo_david_michelle 30:4820042e67b5 58 int shutdown= getLowThrust(-0.3);
ivo_david_michelle 30:4820042e67b5 59 if (shutdown==1) {
ivo_david_michelle 30:4820042e67b5 60 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 61 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 30:4820042e67b5 62 shutDownLed = 1;
ivo_david_michelle 30:4820042e67b5 63 break;
ivo_david_michelle 30:4820042e67b5 64 }
ivo_david_michelle 30:4820042e67b5 65
ivo_david_michelle 23:04338a5ef404 66 myQuadcopter.readSensorValues();
ivo_david_michelle 19:39c144ca2a2f 67
ivo_david_michelle 31:d473eacfc271 68 myQuadcopter.controller(timer.read());
ivo_david_michelle 28:61f7356325c3 69 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 18:a00d6b065c6b 70
ivo_david_michelle 28:61f7356325c3 71 motor_1 = motorsPwm.m1;
ivo_david_michelle 28:61f7356325c3 72 motor_2 = motorsPwm.m2;
ivo_david_michelle 28:61f7356325c3 73 motor_3 = motorsPwm.m3;
ivo_david_michelle 28:61f7356325c3 74 motor_4 = motorsPwm.m4;
ivo_david_michelle 26:7f50323c0c0d 75
ivo_david_michelle 22:92401a4fec13 76 // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r", motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
ivo_david_michelle 15:90e07946186f 77 }
ivo_david_michelle 14:64b06476d943 78 }
ivo_david_michelle 9:f1bd96708a21 79
ivo_david_michelle 15:90e07946186f 80 void rc_thread(void const *args)
ivo_david_michelle 15:90e07946186f 81 {
ivo_david_michelle 15:90e07946186f 82 while(true) {
ivo_david_michelle 15:90e07946186f 83 myQuadcopter.readRc();
ivo_david_michelle 15:90e07946186f 84 }
ivo_david_michelle 14:64b06476d943 85 }
ivo_david_michelle 14:64b06476d943 86
ivo_david_michelle 22:92401a4fec13 87 void battery_thread(void const *args)
ivo_david_michelle 22:92401a4fec13 88 {
ivo_david_michelle 22:92401a4fec13 89 float threshold_voltage = 13.0; // desired lowest battery voltage
ivo_david_michelle 26:7f50323c0c0d 90 float emergencyVoltage = 12.5; // switch off motors below it
ivo_david_michelle 22:92401a4fec13 91 float max_voltage = 14.8; // max voltage level of battery
ivo_david_michelle 22:92401a4fec13 92 float saturating_voltage = 18.38; // voltage at which ADC == 1
ivo_david_michelle 22:92401a4fec13 93 float max_adc = 0.80522; // when battery is at 14.8V
ivo_david_michelle 22:92401a4fec13 94 float threshold_adc = max_adc * threshold_voltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 95 float emergency_adc = max_adc * emergencyVoltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 96
ivo_david_michelle 22:92401a4fec13 97 while(true) {
ivo_david_michelle 22:92401a4fec13 98 if (battery.read() < threshold_adc) {
ivo_david_michelle 22:92401a4fec13 99 printf("low battery! %f\r\n", battery.read() * saturating_voltage);
ivo_david_michelle 29:ae765492fa8b 100 batteryLed = 1;
ivo_david_michelle 26:7f50323c0c0d 101 if (battery.read() < emergency_adc) {
ivo_david_michelle 30:4820042e67b5 102 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 103 break;
ivo_david_michelle 26:7f50323c0c0d 104 }
ivo_david_michelle 22:92401a4fec13 105 }
ivo_david_michelle 22:92401a4fec13 106 Thread::wait(1000); // wait for some number of miliseconds
ivo_david_michelle 22:92401a4fec13 107 }
ivo_david_michelle 22:92401a4fec13 108 }
ivo_david_michelle 22:92401a4fec13 109
ivo_david_michelle 22:92401a4fec13 110
ivo_david_michelle 14:64b06476d943 111
ivo_david_michelle 15:90e07946186f 112 int main()
ivo_david_michelle 15:90e07946186f 113 {
ivo_david_michelle 29:ae765492fa8b 114 // ESCs requires a 100Hz frequency
ivo_david_michelle 29:ae765492fa8b 115 motor_1.period(0.01);
ivo_david_michelle 29:ae765492fa8b 116 motor_2.period(0.01);
ivo_david_michelle 29:ae765492fa8b 117 motor_3.period(0.01);
ivo_david_michelle 29:ae765492fa8b 118 motor_4.period(0.01);
ivo_david_michelle 22:92401a4fec13 119
ivo_david_michelle 27:11116aa69f32 120 Thread threadR(rc_thread);
ivo_david_michelle 30:4820042e67b5 121
ivo_david_michelle 30:4820042e67b5 122
ivo_david_michelle 30:4820042e67b5 123 int startLoop= 0;
ivo_david_michelle 28:61f7356325c3 124
ivo_david_michelle 30:4820042e67b5 125 while (!startLoop) { // wait until Joystick is in starting position
ivo_david_michelle 30:4820042e67b5 126 startLoop= getLowThrust(-0.3);
ivo_david_michelle 27:11116aa69f32 127 }
ivo_david_michelle 30:4820042e67b5 128 nLowThrust = 0; // reset for later use in controller.
ivo_david_michelle 27:11116aa69f32 129
ivo_david_michelle 22:92401a4fec13 130 motor_1 = 0.1;
ivo_david_michelle 22:92401a4fec13 131 motor_2 = 0.1;
ivo_david_michelle 22:92401a4fec13 132 motor_3 = 0.1;
ivo_david_michelle 22:92401a4fec13 133 motor_4 = 0.1;
ivo_david_michelle 4:3040d0f9e8c6 134
ivo_david_michelle 30:4820042e67b5 135 wait(3); // hold startup duty cycle for 2 seconds
ivo_david_michelle 31:d473eacfc271 136
ivo_david_michelle 31:d473eacfc271 137 timer.start();
ivo_david_michelle 22:92401a4fec13 138
ivo_david_michelle 29:ae765492fa8b 139 // TODO assign priorities to threads, test if it really works as we expect
ivo_david_michelle 29:ae765492fa8b 140 Thread battery(battery_thread);
ivo_david_michelle 23:04338a5ef404 141 Thread thread(controller_thread);
ivo_david_michelle 14:64b06476d943 142
ivo_david_michelle 29:ae765492fa8b 143 // TODO is this needed?
ivo_david_michelle 29:ae765492fa8b 144 while (1);
ivo_david_michelle 0:4c04e4fd1310 145 }