David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@46:4bcf2e679f96, 2016-05-05 (annotated)

Committer:
ivo_david_michelle
Date:
Thu May 05 15:58:49 2016 +0000
Revision:
46:4bcf2e679f96
Parent:
45:9f74298eee78
Child:
47:ae478c380136
best stand so far. hovering see video

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 36:40b134328376 10 Timer timer; // timer ;)
ivo_david_michelle 46:4bcf2e679f96 11 //Mutex desired_mutex;
ivo_david_michelle 37:a983eb9fd9c5 12
ivo_david_michelle 37:a983eb9fd9c5 13 int started = 0;
ivo_david_michelle 37:a983eb9fd9c5 14
ivo_david_michelle 46:4bcf2e679f96 15 Quadcopter myQuadcopter(&pc, &mrf, &timer); // instantiate Quadcopter object
ivo_david_michelle 7:f3f94eadc5b5 16
ivo_david_michelle 6:6f3ffd97d808 17 // 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 18 PwmOut motor_1(p23);
ivo_david_michelle 15:90e07946186f 19 PwmOut motor_2(p24);
ivo_david_michelle 15:90e07946186f 20 PwmOut motor_3(p25);
ivo_david_michelle 15:90e07946186f 21 PwmOut motor_4(p26);
ivo_david_michelle 15:90e07946186f 22
ivo_david_michelle 22:92401a4fec13 23 // to read the battery voltage
ivo_david_michelle 22:92401a4fec13 24 AnalogIn battery(p20);
ivo_david_michelle 26:7f50323c0c0d 25 DigitalOut batteryLed(LED1);
ivo_david_michelle 30:4820042e67b5 26 DigitalOut shutDownLed(LED2);
ivo_david_michelle 46:4bcf2e679f96 27 //DigitalOut quicknessLed(LED3); // used for check delay in IMU measurement
ivo_david_michelle 26:7f50323c0c0d 28
ivo_david_michelle 28:61f7356325c3 29 int emergencyOff = 0;
ivo_david_michelle 30:4820042e67b5 30 int nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 31
ivo_david_michelle 46:4bcf2e679f96 32 //void print_count(int id)
ivo_david_michelle 46:4bcf2e679f96 33 //{
ivo_david_michelle 46:4bcf2e679f96 34 // static int count = 0;
ivo_david_michelle 46:4bcf2e679f96 35 // if (count % 1000 == 0) {
ivo_david_michelle 46:4bcf2e679f96 36 // printf("%d\t%d\r\n", id, count);
ivo_david_michelle 46:4bcf2e679f96 37 // }
ivo_david_michelle 46:4bcf2e679f96 38 // count++;
ivo_david_michelle 46:4bcf2e679f96 39 //}
ivo_david_michelle 37:a983eb9fd9c5 40
ivo_david_michelle 30:4820042e67b5 41 void emergencyShutdown()
ivo_david_michelle 30:4820042e67b5 42 {
ivo_david_michelle 30:4820042e67b5 43 emergencyOff = 1;
ivo_david_michelle 42:d09dec5bb184 44 motor_1 = OFF_PWM;
ivo_david_michelle 42:d09dec5bb184 45 motor_2 = OFF_PWM;
ivo_david_michelle 42:d09dec5bb184 46 motor_3 = OFF_PWM;
ivo_david_michelle 42:d09dec5bb184 47 motor_4 = OFF_PWM;
ivo_david_michelle 30:4820042e67b5 48 }
ivo_david_michelle 30:4820042e67b5 49
ivo_david_michelle 30:4820042e67b5 50 int getLowThrust(double threshold)
ivo_david_michelle 30:4820042e67b5 51 {
ivo_david_michelle 30:4820042e67b5 52 double force = myQuadcopter.getForce();
ivo_david_michelle 37:a983eb9fd9c5 53 //printf("%f\r\n", force);
ivo_david_michelle 30:4820042e67b5 54 if (force < threshold) { // if low thrust signal is detected
ivo_david_michelle 30:4820042e67b5 55 nLowThrust++;
ivo_david_michelle 30:4820042e67b5 56 printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
ivo_david_michelle 30:4820042e67b5 57 if (nLowThrust > 5) {
ivo_david_michelle 30:4820042e67b5 58 return 1;
ivo_david_michelle 30:4820042e67b5 59 }
ivo_david_michelle 30:4820042e67b5 60 } else {
ivo_david_michelle 30:4820042e67b5 61 nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 62 }
ivo_david_michelle 30:4820042e67b5 63 return 0;
ivo_david_michelle 30:4820042e67b5 64 }
ivo_david_michelle 4:3040d0f9e8c6 65
ivo_david_michelle 46:4bcf2e679f96 66 /*void controller_thread(void const *args)
ivo_david_michelle 15:90e07946186f 67 {
ivo_david_michelle 26:7f50323c0c0d 68 while(emergencyOff != 1) {
ivo_david_michelle 38:14bf11115f9f 69 // printf(" thrust: %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 37:a983eb9fd9c5 70 myQuadcopter.readSensorValues();
ivo_david_michelle 30:4820042e67b5 71
ivo_david_michelle 37:a983eb9fd9c5 72 myQuadcopter.controller();
ivo_david_michelle 37:a983eb9fd9c5 73 static int count = 0;
ivo_david_michelle 37:a983eb9fd9c5 74 if (count % 100 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 75 printf("%s\t%d\r\n", "controller", count);
ivo_david_michelle 37:a983eb9fd9c5 76 }
ivo_david_michelle 37:a983eb9fd9c5 77 count++;
ivo_david_michelle 37:a983eb9fd9c5 78 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 37:a983eb9fd9c5 79
ivo_david_michelle 37:a983eb9fd9c5 80 motor_1 = motorsPwm.m1;
ivo_david_michelle 37:a983eb9fd9c5 81 motor_2 = motorsPwm.m2;
ivo_david_michelle 37:a983eb9fd9c5 82 motor_3 = motorsPwm.m3;
ivo_david_michelle 37:a983eb9fd9c5 83 motor_4 = motorsPwm.m4;
ivo_david_michelle 37:a983eb9fd9c5 84
ivo_david_michelle 37:a983eb9fd9c5 85 Thread::wait(10);
ivo_david_michelle 37:a983eb9fd9c5 86 Thread::yield();
ivo_david_michelle 37:a983eb9fd9c5 87 // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r", motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
ivo_david_michelle 37:a983eb9fd9c5 88 }
ivo_david_michelle 37:a983eb9fd9c5 89 }
ivo_david_michelle 46:4bcf2e679f96 90 */
ivo_david_michelle 37:a983eb9fd9c5 91
ivo_david_michelle 46:4bcf2e679f96 92 /*
ivo_david_michelle 37:a983eb9fd9c5 93 void rc_thread(void const *args)
ivo_david_michelle 37:a983eb9fd9c5 94 {
ivo_david_michelle 37:a983eb9fd9c5 95 static int count = 0;
ivo_david_michelle 37:a983eb9fd9c5 96 while(true) {
ivo_david_michelle 37:a983eb9fd9c5 97 if (count % 1000 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 98 printf("%s\t%d\r\n", "rc", count);
ivo_david_michelle 37:a983eb9fd9c5 99 }
ivo_david_michelle 38:14bf11115f9f 100 count++;
ivo_david_michelle 37:a983eb9fd9c5 101 myQuadcopter.readRc();
ivo_david_michelle 37:a983eb9fd9c5 102
ivo_david_michelle 37:a983eb9fd9c5 103 if (!started) {
ivo_david_michelle 38:14bf11115f9f 104 continue;
ivo_david_michelle 37:a983eb9fd9c5 105 }
ivo_david_michelle 34:eaea0ae92dfa 106 int shutdown = getLowThrust(-0.3);
ivo_david_michelle 30:4820042e67b5 107 if (shutdown==1) {
ivo_david_michelle 30:4820042e67b5 108 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 109 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 30:4820042e67b5 110 shutDownLed = 1;
ivo_david_michelle 30:4820042e67b5 111 break;
ivo_david_michelle 30:4820042e67b5 112 }
ivo_david_michelle 37:a983eb9fd9c5 113 Thread::wait(10);
ivo_david_michelle 37:a983eb9fd9c5 114 Thread::yield();
ivo_david_michelle 15:90e07946186f 115 }
ivo_david_michelle 14:64b06476d943 116 }
ivo_david_michelle 46:4bcf2e679f96 117 */
ivo_david_michelle 42:d09dec5bb184 118 bool low_battery()
ivo_david_michelle 22:92401a4fec13 119 {
ivo_david_michelle 22:92401a4fec13 120 float threshold_voltage = 13.0; // desired lowest battery voltage
ivo_david_michelle 26:7f50323c0c0d 121 float emergencyVoltage = 12.5; // switch off motors below it
ivo_david_michelle 22:92401a4fec13 122 float max_voltage = 14.8; // max voltage level of battery
ivo_david_michelle 22:92401a4fec13 123 float saturating_voltage = 18.38; // voltage at which ADC == 1
ivo_david_michelle 22:92401a4fec13 124 float max_adc = 0.80522; // when battery is at 14.8V
ivo_david_michelle 22:92401a4fec13 125 float threshold_adc = max_adc * threshold_voltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 126 float emergency_adc = max_adc * emergencyVoltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 127
ivo_david_michelle 42:d09dec5bb184 128 if (battery.read() < threshold_adc) {
ivo_david_michelle 42:d09dec5bb184 129 printf("low battery! %f\r\n", battery.read() * saturating_voltage);
ivo_david_michelle 42:d09dec5bb184 130 batteryLed = 1;
ivo_david_michelle 42:d09dec5bb184 131 if (battery.read() < emergency_adc) {
ivo_david_michelle 42:d09dec5bb184 132 emergencyShutdown();
ivo_david_michelle 42:d09dec5bb184 133 return true;
ivo_david_michelle 37:a983eb9fd9c5 134 }
ivo_david_michelle 22:92401a4fec13 135 }
ivo_david_michelle 42:d09dec5bb184 136 return false;
ivo_david_michelle 22:92401a4fec13 137 }
ivo_david_michelle 22:92401a4fec13 138
ivo_david_michelle 46:4bcf2e679f96 139 //void counting_thread1(void const *args)
ivo_david_michelle 46:4bcf2e679f96 140 //{
ivo_david_michelle 46:4bcf2e679f96 141 // while (true) {
ivo_david_michelle 46:4bcf2e679f96 142 // print_count(1);
ivo_david_michelle 46:4bcf2e679f96 143 // Thread::wait(1);
ivo_david_michelle 46:4bcf2e679f96 144 // }
ivo_david_michelle 46:4bcf2e679f96 145 //}
ivo_david_michelle 46:4bcf2e679f96 146 //
ivo_david_michelle 46:4bcf2e679f96 147 //void counting_thread2(void const *args)
ivo_david_michelle 46:4bcf2e679f96 148 //{
ivo_david_michelle 46:4bcf2e679f96 149 // while (true) {
ivo_david_michelle 46:4bcf2e679f96 150 // print_count(2);
ivo_david_michelle 46:4bcf2e679f96 151 // }
ivo_david_michelle 46:4bcf2e679f96 152 //}
ivo_david_michelle 37:a983eb9fd9c5 153
ivo_david_michelle 15:90e07946186f 154 int main()
ivo_david_michelle 15:90e07946186f 155 {
ivo_david_michelle 29:ae765492fa8b 156 // ESCs requires a 100Hz frequency
ivo_david_michelle 29:ae765492fa8b 157 motor_1.period(0.01);
ivo_david_michelle 29:ae765492fa8b 158 motor_2.period(0.01);
ivo_david_michelle 29:ae765492fa8b 159 motor_3.period(0.01);
ivo_david_michelle 29:ae765492fa8b 160 motor_4.period(0.01);
ivo_david_michelle 22:92401a4fec13 161
ivo_david_michelle 37:a983eb9fd9c5 162 //Thread counting1(counting_thread1);
ivo_david_michelle 37:a983eb9fd9c5 163 //Thread counting2(counting_thread2);
ivo_david_michelle 37:a983eb9fd9c5 164 //counting1.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 165 //counting2.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 166 //Thread thread_rc(rc_thread);
ivo_david_michelle 28:61f7356325c3 167
ivo_david_michelle 46:4bcf2e679f96 168 printf("Waiting for start signal!\r\n");
ivo_david_michelle 40:09a59d5b7944 169
ivo_david_michelle 37:a983eb9fd9c5 170 while (!started) { // wait until Joystick is in starting position
ivo_david_michelle 37:a983eb9fd9c5 171 myQuadcopter.readRc();
ivo_david_michelle 37:a983eb9fd9c5 172 started = getLowThrust(-0.3);
ivo_david_michelle 27:11116aa69f32 173 }
ivo_david_michelle 37:a983eb9fd9c5 174 printf("Started!\r\n");
ivo_david_michelle 30:4820042e67b5 175 nLowThrust = 0; // reset for later use in controller.
ivo_david_michelle 46:4bcf2e679f96 176 // start ESCs up with 10% duty cycle
ivo_david_michelle 46:4bcf2e679f96 177 motor_1 = OFF_PWM;
ivo_david_michelle 46:4bcf2e679f96 178 motor_2 = OFF_PWM;
ivo_david_michelle 46:4bcf2e679f96 179 motor_3 = OFF_PWM;
ivo_david_michelle 46:4bcf2e679f96 180 motor_4 = OFF_PWM;
ivo_david_michelle 27:11116aa69f32 181
ivo_david_michelle 46:4bcf2e679f96 182 wait(1); // hold startup duty cycle for 1 second
ivo_david_michelle 42:d09dec5bb184 183
ivo_david_michelle 46:4bcf2e679f96 184 // Idle thrust
ivo_david_michelle 42:d09dec5bb184 185 motor_1 = MIN_PWM_1; // starts at 11.72% duty cycle
ivo_david_michelle 42:d09dec5bb184 186 motor_2 = MIN_PWM_2; // starts at 11.61% duty cycle
ivo_david_michelle 42:d09dec5bb184 187 motor_3 = MIN_PWM_3; // starts at 11.61% duty cycle
ivo_david_michelle 42:d09dec5bb184 188 motor_4 = MIN_PWM_4; // starts at 11.71% duty cycle
ivo_david_michelle 42:d09dec5bb184 189
ivo_david_michelle 46:4bcf2e679f96 190 wait(1); // hold idle thrust for 1 second
ivo_david_michelle 22:92401a4fec13 191
ivo_david_michelle 29:ae765492fa8b 192 // TODO assign priorities to threads, test if it really works as we expect
ivo_david_michelle 37:a983eb9fd9c5 193 //Thread battery(battery_thread);
ivo_david_michelle 37:a983eb9fd9c5 194 //Thread controller(controller_thread);
ivo_david_michelle 29:ae765492fa8b 195 // TODO is this needed?
ivo_david_michelle 37:a983eb9fd9c5 196 //controller.set_priority(osPriorityRealtime);
ivo_david_michelle 37:a983eb9fd9c5 197 //thread_rc.set_priority(osPriorityRealtime);
ivo_david_michelle 39:fff0a72633ee 198
ivo_david_michelle 38:14bf11115f9f 199 int i = 0;
ivo_david_michelle 43:0e98d5488bf2 200 float previousTime = 0;
ivo_david_michelle 38:14bf11115f9f 201 float currentTime = 0;
ivo_david_michelle 43:0e98d5488bf2 202 float checkBattery = 0.0;
ivo_david_michelle 46:4bcf2e679f96 203 int shutdown = 0;
ivo_david_michelle 46:4bcf2e679f96 204
ivo_david_michelle 46:4bcf2e679f96 205 // Enter while loop with control
ivo_david_michelle 37:a983eb9fd9c5 206 while (1) {
ivo_david_michelle 46:4bcf2e679f96 207 // Check battery level
ivo_david_michelle 37:a983eb9fd9c5 208 i++;
ivo_david_michelle 43:0e98d5488bf2 209 currentTime = timer.read();
ivo_david_michelle 43:0e98d5488bf2 210 checkBattery += currentTime - previousTime;
ivo_david_michelle 45:9f74298eee78 211 if (checkBattery > 1.0) {
ivo_david_michelle 45:9f74298eee78 212 checkBattery = 0.0;
ivo_david_michelle 46:4bcf2e679f96 213 // if (low_battery()) {
ivo_david_michelle 46:4bcf2e679f96 214 // break;
ivo_david_michelle 46:4bcf2e679f96 215 // }
ivo_david_michelle 45:9f74298eee78 216 }
ivo_david_michelle 43:0e98d5488bf2 217 previousTime = currentTime;
ivo_david_michelle 38:14bf11115f9f 218
ivo_david_michelle 39:fff0a72633ee 219 // can be used to measure frequency
ivo_david_michelle 43:0e98d5488bf2 220 // TODO why did it get to 430Hz
ivo_david_michelle 43:0e98d5488bf2 221 // if (i % 1000 == 0) {
ivo_david_michelle 43:0e98d5488bf2 222 // currentTime = timer.read();
ivo_david_michelle 43:0e98d5488bf2 223 // printf("%d, %f, %f\r\n", i, 1000.0 / (currentTime - previousTime), currentTime);
ivo_david_michelle 43:0e98d5488bf2 224 // previousTime = currentTime;
ivo_david_michelle 43:0e98d5488bf2 225 //
ivo_david_michelle 43:0e98d5488bf2 226 // }
ivo_david_michelle 43:0e98d5488bf2 227
ivo_david_michelle 37:a983eb9fd9c5 228 myQuadcopter.readRc();
ivo_david_michelle 42:d09dec5bb184 229
ivo_david_michelle 46:4bcf2e679f96 230 // shutdown
ivo_david_michelle 46:4bcf2e679f96 231 shutdown = getLowThrust(-0.3);
ivo_david_michelle 37:a983eb9fd9c5 232 if (shutdown == 1) {
ivo_david_michelle 37:a983eb9fd9c5 233 emergencyShutdown();
ivo_david_michelle 37:a983eb9fd9c5 234 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 37:a983eb9fd9c5 235 shutDownLed = 1;
ivo_david_michelle 37:a983eb9fd9c5 236 break;
ivo_david_michelle 37:a983eb9fd9c5 237 }
ivo_david_michelle 46:4bcf2e679f96 238 // read sensor values
ivo_david_michelle 37:a983eb9fd9c5 239 myQuadcopter.readSensorValues();
ivo_david_michelle 46:4bcf2e679f96 240 // compute controller and get desired pwm
ivo_david_michelle 37:a983eb9fd9c5 241 myQuadcopter.controller();
ivo_david_michelle 37:a983eb9fd9c5 242 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 46:4bcf2e679f96 243 // set pwm
ivo_david_michelle 37:a983eb9fd9c5 244 motor_1 = motorsPwm.m1;
ivo_david_michelle 37:a983eb9fd9c5 245 motor_2 = motorsPwm.m2;
ivo_david_michelle 37:a983eb9fd9c5 246 motor_3 = motorsPwm.m3;
ivo_david_michelle 37:a983eb9fd9c5 247 motor_4 = motorsPwm.m4;
ivo_david_michelle 37:a983eb9fd9c5 248 }
ivo_david_michelle 0:4c04e4fd1310 249 }