ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

Committer:
ivo_david_michelle
Date:
Sun May 01 22:13:01 2016 +0000
Revision:
39:fff0a72633ee
Parent:
38:14bf11115f9f
Child:
40:09a59d5b7944
some general code clean up

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 30:4820042e67b5 45 motor_1 = 0.1;
ivo_david_michelle 30:4820042e67b5 46 motor_2 = 0.1;
ivo_david_michelle 30:4820042e67b5 47 motor_3 = 0.1;
ivo_david_michelle 30:4820042e67b5 48 motor_4 = 0.1;
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 22:92401a4fec13 117 void battery_thread(void const *args)
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 37:a983eb9fd9c5 127 static int count = 0;
ivo_david_michelle 22:92401a4fec13 128 while(true) {
ivo_david_michelle 37:a983eb9fd9c5 129 if (count % 1 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 130 //printf("%s\t%d\r\n", "battery", count);
ivo_david_michelle 37:a983eb9fd9c5 131 }
ivo_david_michelle 38:14bf11115f9f 132 count++;
ivo_david_michelle 22:92401a4fec13 133 if (battery.read() < threshold_adc) {
ivo_david_michelle 22:92401a4fec13 134 printf("low battery! %f\r\n", battery.read() * saturating_voltage);
ivo_david_michelle 29:ae765492fa8b 135 batteryLed = 1;
ivo_david_michelle 26:7f50323c0c0d 136 if (battery.read() < emergency_adc) {
ivo_david_michelle 30:4820042e67b5 137 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 138 break;
ivo_david_michelle 26:7f50323c0c0d 139 }
ivo_david_michelle 22:92401a4fec13 140 }
ivo_david_michelle 22:92401a4fec13 141 Thread::wait(1000); // wait for some number of miliseconds
ivo_david_michelle 22:92401a4fec13 142 }
ivo_david_michelle 22:92401a4fec13 143 }
ivo_david_michelle 22:92401a4fec13 144
ivo_david_michelle 38:14bf11115f9f 145 void counting_thread1(void const *args)
ivo_david_michelle 38:14bf11115f9f 146 {
ivo_david_michelle 37:a983eb9fd9c5 147 while (true) {
ivo_david_michelle 37:a983eb9fd9c5 148 print_count(1);
ivo_david_michelle 37:a983eb9fd9c5 149 Thread::wait(1);
ivo_david_michelle 37:a983eb9fd9c5 150 }
ivo_david_michelle 37:a983eb9fd9c5 151 }
ivo_david_michelle 37:a983eb9fd9c5 152
ivo_david_michelle 38:14bf11115f9f 153 void counting_thread2(void const *args)
ivo_david_michelle 38:14bf11115f9f 154 {
ivo_david_michelle 37:a983eb9fd9c5 155 while (true) {
ivo_david_michelle 37:a983eb9fd9c5 156 print_count(2);
ivo_david_michelle 37:a983eb9fd9c5 157 }
ivo_david_michelle 37:a983eb9fd9c5 158 }
ivo_david_michelle 37:a983eb9fd9c5 159
ivo_david_michelle 15:90e07946186f 160 int main()
ivo_david_michelle 15:90e07946186f 161 {
ivo_david_michelle 29:ae765492fa8b 162 // ESCs requires a 100Hz frequency
ivo_david_michelle 29:ae765492fa8b 163 motor_1.period(0.01);
ivo_david_michelle 29:ae765492fa8b 164 motor_2.period(0.01);
ivo_david_michelle 29:ae765492fa8b 165 motor_3.period(0.01);
ivo_david_michelle 29:ae765492fa8b 166 motor_4.period(0.01);
ivo_david_michelle 22:92401a4fec13 167
ivo_david_michelle 37:a983eb9fd9c5 168 //Thread counting1(counting_thread1);
ivo_david_michelle 37:a983eb9fd9c5 169 //Thread counting2(counting_thread2);
ivo_david_michelle 37:a983eb9fd9c5 170 //counting1.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 171 //counting2.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 172 //Thread thread_rc(rc_thread);
ivo_david_michelle 28:61f7356325c3 173
ivo_david_michelle 37:a983eb9fd9c5 174 while (!started) { // wait until Joystick is in starting position
ivo_david_michelle 37:a983eb9fd9c5 175 myQuadcopter.readRc();
ivo_david_michelle 37:a983eb9fd9c5 176 started = getLowThrust(-0.3);
ivo_david_michelle 27:11116aa69f32 177 }
ivo_david_michelle 37:a983eb9fd9c5 178 printf("Started!\r\n");
ivo_david_michelle 30:4820042e67b5 179 nLowThrust = 0; // reset for later use in controller.
ivo_david_michelle 27:11116aa69f32 180
ivo_david_michelle 22:92401a4fec13 181 motor_1 = 0.1;
ivo_david_michelle 22:92401a4fec13 182 motor_2 = 0.1;
ivo_david_michelle 22:92401a4fec13 183 motor_3 = 0.1;
ivo_david_michelle 22:92401a4fec13 184 motor_4 = 0.1;
ivo_david_michelle 4:3040d0f9e8c6 185
ivo_david_michelle 30:4820042e67b5 186 wait(3); // hold startup duty cycle for 2 seconds
ivo_david_michelle 22:92401a4fec13 187
ivo_david_michelle 29:ae765492fa8b 188 // TODO assign priorities to threads, test if it really works as we expect
ivo_david_michelle 37:a983eb9fd9c5 189 //Thread battery(battery_thread);
ivo_david_michelle 37:a983eb9fd9c5 190 //Thread controller(controller_thread);
ivo_david_michelle 29:ae765492fa8b 191 // TODO is this needed?
ivo_david_michelle 38:14bf11115f9f 192
ivo_david_michelle 37:a983eb9fd9c5 193 //controller.set_priority(osPriorityRealtime);
ivo_david_michelle 37:a983eb9fd9c5 194 //thread_rc.set_priority(osPriorityRealtime);
ivo_david_michelle 39:fff0a72633ee 195
ivo_david_michelle 38:14bf11115f9f 196 int i = 0;
ivo_david_michelle 38:14bf11115f9f 197 float previousTime =0;
ivo_david_michelle 38:14bf11115f9f 198 float currentTime = 0;
ivo_david_michelle 37:a983eb9fd9c5 199 while (1) {
ivo_david_michelle 37:a983eb9fd9c5 200 i++;
ivo_david_michelle 38:14bf11115f9f 201
ivo_david_michelle 39:fff0a72633ee 202 // can be used to measure frequency
ivo_david_michelle 39:fff0a72633ee 203 /*
ivo_david_michelle 39:fff0a72633ee 204 if (i % 5000 == 0) {
ivo_david_michelle 38:14bf11115f9f 205 currentTime = timer.read();
ivo_david_michelle 38:14bf11115f9f 206 printf("%d, %f, %f\r\n", i, 5000.0 / (currentTime - previousTime), currentTime);
ivo_david_michelle 38:14bf11115f9f 207 previousTime = currentTime;
ivo_david_michelle 38:14bf11115f9f 208
ivo_david_michelle 39:fff0a72633ee 209 }
ivo_david_michelle 39:fff0a72633ee 210 */
ivo_david_michelle 38:14bf11115f9f 211
ivo_david_michelle 37:a983eb9fd9c5 212 myQuadcopter.readRc();
ivo_david_michelle 38:14bf11115f9f 213
ivo_david_michelle 37:a983eb9fd9c5 214 int shutdown = getLowThrust(-0.3);
ivo_david_michelle 37:a983eb9fd9c5 215 if (shutdown == 1) {
ivo_david_michelle 37:a983eb9fd9c5 216 emergencyShutdown();
ivo_david_michelle 37:a983eb9fd9c5 217 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 37:a983eb9fd9c5 218 shutDownLed = 1;
ivo_david_michelle 37:a983eb9fd9c5 219 break;
ivo_david_michelle 37:a983eb9fd9c5 220 }
ivo_david_michelle 37:a983eb9fd9c5 221 myQuadcopter.readSensorValues();
ivo_david_michelle 39:fff0a72633ee 222
ivo_david_michelle 39:fff0a72633ee 223 // can be used to test responsiveness of the IMU
ivo_david_michelle 39:fff0a72633ee 224 /*
ivo_david_michelle 39:fff0a72633ee 225 state mystate = myQuadcopter.getState();
ivo_david_michelle 38:14bf11115f9f 226 if (mystate.phi > 0.0785) {
ivo_david_michelle 38:14bf11115f9f 227 quicknessLed = 1;
ivo_david_michelle 38:14bf11115f9f 228 } else {
ivo_david_michelle 38:14bf11115f9f 229 quicknessLed = 0;
ivo_david_michelle 39:fff0a72633ee 230 }
ivo_david_michelle 39:fff0a72633ee 231 */
ivo_david_michelle 38:14bf11115f9f 232
ivo_david_michelle 37:a983eb9fd9c5 233 myQuadcopter.controller();
ivo_david_michelle 37:a983eb9fd9c5 234 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 37:a983eb9fd9c5 235 motor_1 = motorsPwm.m1;
ivo_david_michelle 37:a983eb9fd9c5 236 motor_2 = motorsPwm.m2;
ivo_david_michelle 37:a983eb9fd9c5 237 motor_3 = motorsPwm.m3;
ivo_david_michelle 37:a983eb9fd9c5 238 motor_4 = motorsPwm.m4;
ivo_david_michelle 37:a983eb9fd9c5 239 }
ivo_david_michelle 0:4c04e4fd1310 240 }