David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

main.cpp@27:11116aa69f32, 2016-04-14 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Apr 14 22:32:30 2016 +0000
Revision:
27:11116aa69f32
Parent:
26:7f50323c0c0d
Child:
28:61f7356325c3
changed control gains, implemented cut off at 15% duty cycle

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 26:7f50323c0c0d 21
ivo_david_michelle 26:7f50323c0c0d 22
ivo_david_michelle 26:7f50323c0c0d 23 //
ivo_david_michelle 26:7f50323c0c0d 24 int emergencyOff=0;
ivo_david_michelle 4:3040d0f9e8c6 25
ivo_david_michelle 15:90e07946186f 26 void controller_thread(void const *args)
ivo_david_michelle 15:90e07946186f 27 {
ivo_david_michelle 26:7f50323c0c0d 28 while(emergencyOff != 1) {
ivo_david_michelle 23:04338a5ef404 29 myQuadcopter.readSensorValues();
ivo_david_michelle 19:39c144ca2a2f 30
ivo_david_michelle 15:90e07946186f 31 myQuadcopter.controller();
ivo_david_michelle 18:a00d6b065c6b 32 motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 18:a00d6b065c6b 33
ivo_david_michelle 21:336faf452989 34 motor_1=motorsPwm.m1;
ivo_david_michelle 21:336faf452989 35 motor_2=motorsPwm.m2;
ivo_david_michelle 21:336faf452989 36 motor_3=motorsPwm.m3;
ivo_david_michelle 18:a00d6b065c6b 37 motor_4=motorsPwm.m4;
ivo_david_michelle 22:92401a4fec13 38
ivo_david_michelle 26:7f50323c0c0d 39
ivo_david_michelle 26:7f50323c0c0d 40
ivo_david_michelle 22:92401a4fec13 41 // pc.printf("m1: %f m2: %f m3: %f m4: %f \n\r", motorsPwm.m1, motorsPwm.m2, motorsPwm.m3, motorsPwm.m4);
ivo_david_michelle 18:a00d6b065c6b 42
ivo_david_michelle 15:90e07946186f 43 Thread::wait(10);
ivo_david_michelle 15:90e07946186f 44 }
ivo_david_michelle 14:64b06476d943 45 }
ivo_david_michelle 9:f1bd96708a21 46
ivo_david_michelle 15:90e07946186f 47 void rc_thread(void const *args)
ivo_david_michelle 15:90e07946186f 48 {
ivo_david_michelle 15:90e07946186f 49 while(true) {
ivo_david_michelle 15:90e07946186f 50 myQuadcopter.readRc();
ivo_david_michelle 18:a00d6b065c6b 51 Thread::wait(50); // wait for some number of miliseconds
ivo_david_michelle 15:90e07946186f 52 }
ivo_david_michelle 14:64b06476d943 53 }
ivo_david_michelle 14:64b06476d943 54
ivo_david_michelle 22:92401a4fec13 55 void battery_thread(void const *args)
ivo_david_michelle 22:92401a4fec13 56 {
ivo_david_michelle 22:92401a4fec13 57 float threshold_voltage = 13.0; // desired lowest battery voltage
ivo_david_michelle 26:7f50323c0c0d 58 float emergencyVoltage = 12.5; // switch off motors below it
ivo_david_michelle 22:92401a4fec13 59 float max_voltage = 14.8; // max voltage level of battery
ivo_david_michelle 22:92401a4fec13 60 float saturating_voltage = 18.38; // voltage at which ADC == 1
ivo_david_michelle 22:92401a4fec13 61 float max_adc = 0.80522; // when battery is at 14.8V
ivo_david_michelle 22:92401a4fec13 62 float threshold_adc = max_adc * threshold_voltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 63 float emergency_adc = max_adc * emergencyVoltage / max_voltage;
ivo_david_michelle 26:7f50323c0c0d 64
ivo_david_michelle 22:92401a4fec13 65 while(true) {
ivo_david_michelle 22:92401a4fec13 66 if (battery.read() < threshold_adc) {
ivo_david_michelle 22:92401a4fec13 67 printf("low battery! %f\r\n", battery.read() * saturating_voltage);
ivo_david_michelle 26:7f50323c0c0d 68 batteryLed=1;
ivo_david_michelle 26:7f50323c0c0d 69 if (battery.read() < emergency_adc) {
ivo_david_michelle 26:7f50323c0c0d 70 emergencyOff=1;
ivo_david_michelle 26:7f50323c0c0d 71 motor_1=0.1;
ivo_david_michelle 26:7f50323c0c0d 72 motor_2=0.1;
ivo_david_michelle 26:7f50323c0c0d 73 motor_3=0.1;
ivo_david_michelle 26:7f50323c0c0d 74 motor_4=0.1;
ivo_david_michelle 26:7f50323c0c0d 75 }
ivo_david_michelle 22:92401a4fec13 76 }
ivo_david_michelle 22:92401a4fec13 77 Thread::wait(1000); // wait for some number of miliseconds
ivo_david_michelle 22:92401a4fec13 78 }
ivo_david_michelle 22:92401a4fec13 79 }
ivo_david_michelle 22:92401a4fec13 80
ivo_david_michelle 22:92401a4fec13 81
ivo_david_michelle 14:64b06476d943 82
ivo_david_michelle 15:90e07946186f 83 int main()
ivo_david_michelle 15:90e07946186f 84 {
ivo_david_michelle 27:11116aa69f32 85
ivo_david_michelle 27:11116aa69f32 86
ivo_david_michelle 14:64b06476d943 87 // get desired values from joystick (to be implemented)
ivo_david_michelle 14:64b06476d943 88 // myQuadcopter.setDes(...)
ivo_david_michelle 10:e7d1801e966a 89
ivo_david_michelle 22:92401a4fec13 90 motor_1.period(0.01); // motor requires a 2ms period
ivo_david_michelle 22:92401a4fec13 91 motor_2.period(0.01); // motor requires a 2ms period
ivo_david_michelle 22:92401a4fec13 92 motor_3.period(0.01); // motor requires a 2ms period
ivo_david_michelle 22:92401a4fec13 93 motor_4.period(0.01); // motor requires a 2ms period
ivo_david_michelle 22:92401a4fec13 94
ivo_david_michelle 27:11116aa69f32 95 Thread threadR(rc_thread);
ivo_david_michelle 27:11116aa69f32 96 double forceThreshold = -0.3;
ivo_david_michelle 27:11116aa69f32 97 double forceRc = myQuadcopter.getForce();
ivo_david_michelle 27:11116aa69f32 98
ivo_david_michelle 27:11116aa69f32 99 while (forceRc > forceThreshold) // wait until Joystick is in starting position
ivo_david_michelle 27:11116aa69f32 100 {
ivo_david_michelle 27:11116aa69f32 101 forceRc = myQuadcopter.getForce();
ivo_david_michelle 27:11116aa69f32 102 }
ivo_david_michelle 27:11116aa69f32 103
ivo_david_michelle 27:11116aa69f32 104 motor_1 = 0.1;
ivo_david_michelle 27:11116aa69f32 105 motor_2 = 0.1;
ivo_david_michelle 27:11116aa69f32 106 motor_3 = 0.1;
ivo_david_michelle 27:11116aa69f32 107 motor_4 = 0.1;
ivo_david_michelle 27:11116aa69f32 108
ivo_david_michelle 27:11116aa69f32 109 wait(2); // hold startup duty cycle for 2 seconds
ivo_david_michelle 27:11116aa69f32 110
ivo_david_michelle 27:11116aa69f32 111 /*
ivo_david_michelle 4:3040d0f9e8c6 112 // startup procedure
ivo_david_michelle 4:3040d0f9e8c6 113 pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
ivo_david_michelle 4:3040d0f9e8c6 114 char a= pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 115 if (a!='s') {
ivo_david_michelle 4:3040d0f9e8c6 116 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 117 return 0;
ivo_david_michelle 4:3040d0f9e8c6 118 };
ivo_david_michelle 13:291ba30c7806 119
ivo_david_michelle 12:422963993df5 120 // Duty cycle at beginning must be 50%
ivo_david_michelle 4:3040d0f9e8c6 121 pc.printf("Starting up ESCs\n\r");
ivo_david_michelle 22:92401a4fec13 122 motor_1 = 0.1;
ivo_david_michelle 22:92401a4fec13 123 motor_2 = 0.1;
ivo_david_michelle 22:92401a4fec13 124 motor_3 = 0.1;
ivo_david_michelle 22:92401a4fec13 125 motor_4 = 0.1;
ivo_david_michelle 4:3040d0f9e8c6 126
ivo_david_michelle 4:3040d0f9e8c6 127 pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
ivo_david_michelle 14:64b06476d943 128 char b = pc.getc();
ivo_david_michelle 4:3040d0f9e8c6 129 if (b!='c') {
ivo_david_michelle 4:3040d0f9e8c6 130 pc.printf("Aborting");
ivo_david_michelle 4:3040d0f9e8c6 131 return 0;
ivo_david_michelle 4:3040d0f9e8c6 132 };
ivo_david_michelle 27:11116aa69f32 133 */
ivo_david_michelle 22:92401a4fec13 134 /* pc.printf("Outputting duty cycle specified below now press all but b to abort.\n\r");
ivo_david_michelle 22:92401a4fec13 135 motor_1 = 0.07;
ivo_david_michelle 22:92401a4fec13 136
ivo_david_michelle 22:92401a4fec13 137
ivo_david_michelle 22:92401a4fec13 138 char c = pc.getc();
ivo_david_michelle 22:92401a4fec13 139 if (c!='c') {
ivo_david_michelle 22:92401a4fec13 140 pc.printf("Aborting");
ivo_david_michelle 22:92401a4fec13 141 return 0;
ivo_david_michelle 22:92401a4fec13 142 };
ivo_david_michelle 22:92401a4fec13 143 */
ivo_david_michelle 22:92401a4fec13 144
ivo_david_michelle 25:d44610851105 145 // TODO assign priorities to threads, test if it really works as we expect
ivo_david_michelle 23:04338a5ef404 146 Thread thread(controller_thread);
ivo_david_michelle 26:7f50323c0c0d 147 Thread battery(battery_thread);
ivo_david_michelle 14:64b06476d943 148
ivo_david_michelle 27:11116aa69f32 149 //pc.printf("Entering control loop\n\r");
ivo_david_michelle 4:3040d0f9e8c6 150
ivo_david_michelle 4:3040d0f9e8c6 151 while (1) {
ivo_david_michelle 19:39c144ca2a2f 152 //myQuadcopter.readSensorValues();
ivo_david_michelle 11:5c54826d23a7 153
ivo_david_michelle 15:90e07946186f 154 // myQuadcopter.controller();
ivo_david_michelle 13:291ba30c7806 155
ivo_david_michelle 17:96d0c72e413e 156 // wait(0.01);
ivo_david_michelle 1:b87e95907a18 157
ivo_david_michelle 4:3040d0f9e8c6 158 // Set duty cycle
ivo_david_michelle 22:92401a4fec13 159 // motors motorsPwm=myQuadcopter.getPwm();
ivo_david_michelle 5:f007542f1dab 160
ivo_david_michelle 18:a00d6b065c6b 161 //motor_2=motorsPwm.m2;
ivo_david_michelle 18:a00d6b065c6b 162 //motor_4=motorsPwm.m4;
ivo_david_michelle 15:90e07946186f 163
ivo_david_michelle 14:64b06476d943 164 //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r", F, M_x, M_y, M_z);
ivo_david_michelle 15:90e07946186f 165 // pc.printf("m1: %f m2: %f \n\r", motorsPwm.m2, motorsPwm.m4);
ivo_david_michelle 11:5c54826d23a7 166 //pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
ivo_david_michelle 0:4c04e4fd1310 167 }
ivo_david_michelle 0:4c04e4fd1310 168 }