David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@43:0e98d5488bf2, 2016-05-05 (annotated)

Committer:
ivo_david_michelle
Date:
Thu May 05 03:22:59 2016 +0000
Revision:
43:0e98d5488bf2
Parent:
42:d09dec5bb184
Child:
45:9f74298eee78
before deleting old offsets;

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