David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@37:a983eb9fd9c5, 2016-05-01 (annotated)

Committer:
ivo_david_michelle
Date:
Sun May 01 18:45:02 2016 +0000
Revision:
37:a983eb9fd9c5
Parent:
36:40b134328376
Child:
38:14bf11115f9f
david's changes and printing diagnostics from readRC

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 26:7f50323c0c0d 27
ivo_david_michelle 28:61f7356325c3 28 int emergencyOff = 0;
ivo_david_michelle 30:4820042e67b5 29 //int lowThrust= {0,0,0};
ivo_david_michelle 30:4820042e67b5 30 int nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 31
ivo_david_michelle 37:a983eb9fd9c5 32 void print_count(int id) {
ivo_david_michelle 37:a983eb9fd9c5 33 static int count = 0;
ivo_david_michelle 37:a983eb9fd9c5 34 if (count % 1000 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 35 printf("%d\t%d\r\n", id, count);
ivo_david_michelle 37:a983eb9fd9c5 36 }
ivo_david_michelle 37:a983eb9fd9c5 37 count++;
ivo_david_michelle 37:a983eb9fd9c5 38 }
ivo_david_michelle 37:a983eb9fd9c5 39
ivo_david_michelle 30:4820042e67b5 40 void emergencyShutdown()
ivo_david_michelle 30:4820042e67b5 41 {
ivo_david_michelle 30:4820042e67b5 42 emergencyOff = 1;
ivo_david_michelle 30:4820042e67b5 43 motor_1 = 0.1;
ivo_david_michelle 30:4820042e67b5 44 motor_2 = 0.1;
ivo_david_michelle 30:4820042e67b5 45 motor_3 = 0.1;
ivo_david_michelle 30:4820042e67b5 46 motor_4 = 0.1;
ivo_david_michelle 30:4820042e67b5 47 }
ivo_david_michelle 30:4820042e67b5 48
ivo_david_michelle 30:4820042e67b5 49 int getLowThrust(double threshold)
ivo_david_michelle 30:4820042e67b5 50 {
ivo_david_michelle 30:4820042e67b5 51 double force = myQuadcopter.getForce();
ivo_david_michelle 37:a983eb9fd9c5 52 //printf("%f\r\n", force);
ivo_david_michelle 30:4820042e67b5 53 if (force < threshold) { // if low thrust signal is detected
ivo_david_michelle 30:4820042e67b5 54 nLowThrust++;
ivo_david_michelle 30:4820042e67b5 55 printf("Negative thrust! %f, nLowThrust %d\r\n",myQuadcopter.getForce(),nLowThrust);
ivo_david_michelle 30:4820042e67b5 56 if (nLowThrust > 5) {
ivo_david_michelle 30:4820042e67b5 57 return 1;
ivo_david_michelle 30:4820042e67b5 58 }
ivo_david_michelle 30:4820042e67b5 59 } else {
ivo_david_michelle 30:4820042e67b5 60 nLowThrust = 0;
ivo_david_michelle 30:4820042e67b5 61 }
ivo_david_michelle 30:4820042e67b5 62 return 0;
ivo_david_michelle 30:4820042e67b5 63 }
ivo_david_michelle 4:3040d0f9e8c6 64
ivo_david_michelle 15:90e07946186f 65 void controller_thread(void const *args)
ivo_david_michelle 15:90e07946186f 66 {
ivo_david_michelle 26:7f50323c0c0d 67 while(emergencyOff != 1) {
ivo_david_michelle 30:4820042e67b5 68 // printf(" thrust: %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 37:a983eb9fd9c5 69 myQuadcopter.readSensorValues();
ivo_david_michelle 30:4820042e67b5 70
ivo_david_michelle 37:a983eb9fd9c5 71 myQuadcopter.controller();
ivo_david_michelle 37:a983eb9fd9c5 72 static int count = 0;
ivo_david_michelle 37:a983eb9fd9c5 73 if (count % 100 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 74 printf("%s\t%d\r\n", "controller", count);
ivo_david_michelle 37:a983eb9fd9c5 75 }
ivo_david_michelle 37:a983eb9fd9c5 76 count++;
ivo_david_michelle 37:a983eb9fd9c5 77 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 37:a983eb9fd9c5 78
ivo_david_michelle 37:a983eb9fd9c5 79 motor_1 = motorsPwm.m1;
ivo_david_michelle 37:a983eb9fd9c5 80 motor_2 = motorsPwm.m2;
ivo_david_michelle 37:a983eb9fd9c5 81 motor_3 = motorsPwm.m3;
ivo_david_michelle 37:a983eb9fd9c5 82 motor_4 = motorsPwm.m4;
ivo_david_michelle 37:a983eb9fd9c5 83
ivo_david_michelle 37:a983eb9fd9c5 84 Thread::wait(10);
ivo_david_michelle 37:a983eb9fd9c5 85 Thread::yield();
ivo_david_michelle 37:a983eb9fd9c5 86 // 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 87 }
ivo_david_michelle 37:a983eb9fd9c5 88 }
ivo_david_michelle 37:a983eb9fd9c5 89
ivo_david_michelle 37:a983eb9fd9c5 90 void rc_thread(void const *args)
ivo_david_michelle 37:a983eb9fd9c5 91 {
ivo_david_michelle 37:a983eb9fd9c5 92 static int count = 0;
ivo_david_michelle 37:a983eb9fd9c5 93 while(true) {
ivo_david_michelle 37:a983eb9fd9c5 94 if (count % 1000 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 95 printf("%s\t%d\r\n", "rc", count);
ivo_david_michelle 37:a983eb9fd9c5 96 }
ivo_david_michelle 37:a983eb9fd9c5 97 count++;
ivo_david_michelle 37:a983eb9fd9c5 98 myQuadcopter.readRc();
ivo_david_michelle 37:a983eb9fd9c5 99
ivo_david_michelle 37:a983eb9fd9c5 100 if (!started) {
ivo_david_michelle 37:a983eb9fd9c5 101 continue;
ivo_david_michelle 37:a983eb9fd9c5 102 }
ivo_david_michelle 34:eaea0ae92dfa 103 int shutdown = getLowThrust(-0.3);
ivo_david_michelle 30:4820042e67b5 104 if (shutdown==1) {
ivo_david_michelle 30:4820042e67b5 105 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 106 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 30:4820042e67b5 107 shutDownLed = 1;
ivo_david_michelle 30:4820042e67b5 108 break;
ivo_david_michelle 30:4820042e67b5 109 }
ivo_david_michelle 37:a983eb9fd9c5 110 Thread::wait(10);
ivo_david_michelle 37:a983eb9fd9c5 111 Thread::yield();
ivo_david_michelle 15:90e07946186f 112 }
ivo_david_michelle 14:64b06476d943 113 }
ivo_david_michelle 14:64b06476d943 114
ivo_david_michelle 22:92401a4fec13 115 void battery_thread(void const *args)
ivo_david_michelle 22:92401a4fec13 116 {
ivo_david_michelle 22:92401a4fec13 117 float threshold_voltage = 13.0; // desired lowest battery voltage
ivo_david_michelle 26:7f50323c0c0d 118 float emergencyVoltage = 12.5; // switch off motors below it
ivo_david_michelle 22:92401a4fec13 119 float max_voltage = 14.8; // max voltage level of battery
ivo_david_michelle 22:92401a4fec13 120 float saturating_voltage = 18.38; // voltage at which ADC == 1
ivo_david_michelle 22:92401a4fec13 121 float max_adc = 0.80522; // when battery is at 14.8V
ivo_david_michelle 22:92401a4fec13 122 float threshold_adc = max_adc * threshold_voltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 123 float emergency_adc = max_adc * emergencyVoltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 124
ivo_david_michelle 37:a983eb9fd9c5 125 static int count = 0;
ivo_david_michelle 22:92401a4fec13 126 while(true) {
ivo_david_michelle 37:a983eb9fd9c5 127 if (count % 1 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 128 //printf("%s\t%d\r\n", "battery", count);
ivo_david_michelle 37:a983eb9fd9c5 129 }
ivo_david_michelle 37:a983eb9fd9c5 130 count++;
ivo_david_michelle 22:92401a4fec13 131 if (battery.read() < threshold_adc) {
ivo_david_michelle 22:92401a4fec13 132 printf("low battery! %f\r\n", battery.read() * saturating_voltage);
ivo_david_michelle 29:ae765492fa8b 133 batteryLed = 1;
ivo_david_michelle 26:7f50323c0c0d 134 if (battery.read() < emergency_adc) {
ivo_david_michelle 30:4820042e67b5 135 emergencyShutdown();
ivo_david_michelle 30:4820042e67b5 136 break;
ivo_david_michelle 26:7f50323c0c0d 137 }
ivo_david_michelle 22:92401a4fec13 138 }
ivo_david_michelle 22:92401a4fec13 139 Thread::wait(1000); // wait for some number of miliseconds
ivo_david_michelle 22:92401a4fec13 140 }
ivo_david_michelle 22:92401a4fec13 141 }
ivo_david_michelle 22:92401a4fec13 142
ivo_david_michelle 37:a983eb9fd9c5 143 void counting_thread1(void const *args) {
ivo_david_michelle 37:a983eb9fd9c5 144 while (true) {
ivo_david_michelle 37:a983eb9fd9c5 145 print_count(1);
ivo_david_michelle 37:a983eb9fd9c5 146 Thread::wait(1);
ivo_david_michelle 37:a983eb9fd9c5 147 }
ivo_david_michelle 37:a983eb9fd9c5 148 }
ivo_david_michelle 37:a983eb9fd9c5 149
ivo_david_michelle 37:a983eb9fd9c5 150 void counting_thread2(void const *args) {
ivo_david_michelle 37:a983eb9fd9c5 151 while (true) {
ivo_david_michelle 37:a983eb9fd9c5 152 print_count(2);
ivo_david_michelle 37:a983eb9fd9c5 153 }
ivo_david_michelle 37:a983eb9fd9c5 154 }
ivo_david_michelle 37:a983eb9fd9c5 155
ivo_david_michelle 15:90e07946186f 156 int main()
ivo_david_michelle 15:90e07946186f 157 {
ivo_david_michelle 29:ae765492fa8b 158 // ESCs requires a 100Hz frequency
ivo_david_michelle 29:ae765492fa8b 159 motor_1.period(0.01);
ivo_david_michelle 29:ae765492fa8b 160 motor_2.period(0.01);
ivo_david_michelle 29:ae765492fa8b 161 motor_3.period(0.01);
ivo_david_michelle 29:ae765492fa8b 162 motor_4.period(0.01);
ivo_david_michelle 22:92401a4fec13 163
ivo_david_michelle 37:a983eb9fd9c5 164 //Thread counting1(counting_thread1);
ivo_david_michelle 37:a983eb9fd9c5 165 //Thread counting2(counting_thread2);
ivo_david_michelle 37:a983eb9fd9c5 166 //counting1.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 167 //counting2.set_priority(osPriorityHigh);
ivo_david_michelle 37:a983eb9fd9c5 168 //Thread thread_rc(rc_thread);
ivo_david_michelle 28:61f7356325c3 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 27:11116aa69f32 176
ivo_david_michelle 22:92401a4fec13 177 motor_1 = 0.1;
ivo_david_michelle 22:92401a4fec13 178 motor_2 = 0.1;
ivo_david_michelle 22:92401a4fec13 179 motor_3 = 0.1;
ivo_david_michelle 22:92401a4fec13 180 motor_4 = 0.1;
ivo_david_michelle 4:3040d0f9e8c6 181
ivo_david_michelle 30:4820042e67b5 182 wait(3); // hold startup duty cycle for 2 seconds
ivo_david_michelle 22:92401a4fec13 183
ivo_david_michelle 29:ae765492fa8b 184 // TODO assign priorities to threads, test if it really works as we expect
ivo_david_michelle 37:a983eb9fd9c5 185 //Thread battery(battery_thread);
ivo_david_michelle 37:a983eb9fd9c5 186 //Thread controller(controller_thread);
ivo_david_michelle 29:ae765492fa8b 187 // TODO is this needed?
ivo_david_michelle 37:a983eb9fd9c5 188
ivo_david_michelle 37:a983eb9fd9c5 189 //controller.set_priority(osPriorityRealtime);
ivo_david_michelle 37:a983eb9fd9c5 190 //thread_rc.set_priority(osPriorityRealtime);
ivo_david_michelle 37:a983eb9fd9c5 191 int i =0;
ivo_david_michelle 37:a983eb9fd9c5 192 while (1) {
ivo_david_michelle 37:a983eb9fd9c5 193 i++;
ivo_david_michelle 37:a983eb9fd9c5 194 if (i % 100 == 0) {
ivo_david_michelle 37:a983eb9fd9c5 195 printf("%d\r\n", i);
ivo_david_michelle 37:a983eb9fd9c5 196 }
ivo_david_michelle 37:a983eb9fd9c5 197 myQuadcopter.readRc();
ivo_david_michelle 37:a983eb9fd9c5 198 int shutdown = getLowThrust(-0.3);
ivo_david_michelle 37:a983eb9fd9c5 199 if (shutdown == 1) {
ivo_david_michelle 37:a983eb9fd9c5 200 emergencyShutdown();
ivo_david_michelle 37:a983eb9fd9c5 201 printf("too long negative thrust! %f\r\n",myQuadcopter.getForce());
ivo_david_michelle 37:a983eb9fd9c5 202 shutDownLed = 1;
ivo_david_michelle 37:a983eb9fd9c5 203 break;
ivo_david_michelle 37:a983eb9fd9c5 204 }
ivo_david_michelle 37:a983eb9fd9c5 205 myQuadcopter.readSensorValues();
ivo_david_michelle 37:a983eb9fd9c5 206 myQuadcopter.controller();
ivo_david_michelle 37:a983eb9fd9c5 207 motors motorsPwm = myQuadcopter.getPwm();
ivo_david_michelle 37:a983eb9fd9c5 208 motor_1 = motorsPwm.m1;
ivo_david_michelle 37:a983eb9fd9c5 209 motor_2 = motorsPwm.m2;
ivo_david_michelle 37:a983eb9fd9c5 210 motor_3 = motorsPwm.m3;
ivo_david_michelle 37:a983eb9fd9c5 211 motor_4 = motorsPwm.m4;
ivo_david_michelle 37:a983eb9fd9c5 212 }
ivo_david_michelle 0:4c04e4fd1310 213 }