Active Aerodynamics and Drag Reduction System / Mbed 2 deprecated ActiveAeroAndDRS

Source code for Active Aerodynamics and Drag Reduction System

Dependencies:   mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2

main.cpp@5:295fe3425a73, 2020-04-24 (annotated)

Committer:
skiley6
Date:
Fri Apr 24 15:24:30 2020 +0000
Revision:
5:295fe3425a73
Parent:
4:8442a7d55f23
Child:
6:38cc8e010406
Added Temperature and made sure everything works. Temp is in Main thread;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cheryldocherty 0:04fef978a0ab 1 #include "mbed.h"
cheryldocherty 0:04fef978a0ab 2 #include "rtos.h"
cheryldocherty 0:04fef978a0ab 3 #include "Servo.h"
cheryldocherty 0:04fef978a0ab 4 #include "LSM9DS1.h"
skiley6 5:295fe3425a73 5 #include "MPL3115A2.h"
cheryldocherty 0:04fef978a0ab 6 #define PI 3.14159 // Used in IMU code
cheryldocherty 0:04fef978a0ab 7 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA (Used in IMU code)
cheryldocherty 0:04fef978a0ab 8
cheryldocherty 0:04fef978a0ab 9
skiley6 3:4b9d098dcb04 10 // SETUP
skiley6 3:4b9d098dcb04 11 Servo servoFR(p21);
skiley6 3:4b9d098dcb04 12 Servo servoFL(p22);
skiley6 3:4b9d098dcb04 13 Servo servoRR(p23);
skiley6 3:4b9d098dcb04 14 Servo servoRL(p24);
cheryldocherty 0:04fef978a0ab 15 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
skiley6 3:4b9d098dcb04 16 RawSerial blue(p13,p14); // bluetooth
skiley6 3:4b9d098dcb04 17 Serial pc(USBTX, USBRX); // tx, rx
skiley6 3:4b9d098dcb04 18
skiley6 5:295fe3425a73 19 I2C i2c(p28, p27); // sda, scl
skiley6 5:295fe3425a73 20 MPL3115A2 sensor(&i2c, &pc);
skiley6 5:295fe3425a73 21
cheryldocherty 0:04fef978a0ab 22 //BusOut myled(LED1,LED2,LED3,LED4); //bluetooth debugging
skiley6 3:4b9d098dcb04 23 DigitalOut led1(LED1);
skiley6 3:4b9d098dcb04 24 DigitalOut led2(LED2);
skiley6 3:4b9d098dcb04 25 DigitalOut led3(LED3);
skiley6 3:4b9d098dcb04 26 DigitalOut led4(LED4);
cheryldocherty 0:04fef978a0ab 27
skiley6 3:4b9d098dcb04 28 //THREADS
skiley6 3:4b9d098dcb04 29 Thread blueRXThread;
skiley6 3:4b9d098dcb04 30 Thread blueTXThread;
skiley6 5:295fe3425a73 31 Thread IMUThread;
skiley6 3:4b9d098dcb04 32 Thread servoThread;
cheryldocherty 0:04fef978a0ab 33
cheryldocherty 0:04fef978a0ab 34 // VARIABLE DECLARATIONS
skiley6 3:4b9d098dcb04 35 volatile float YawRate;
skiley6 4:8442a7d55f23 36 volatile float roll;
skiley6 5:295fe3425a73 37
skiley6 5:295fe3425a73 38 Temperature t;
skiley6 5:295fe3425a73 39
cheryldocherty 0:04fef978a0ab 40
skiley6 3:4b9d098dcb04 41 enum statetype {Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll};
skiley6 3:4b9d098dcb04 42 volatile statetype servoState = Off;
skiley6 4:8442a7d55f23 43 volatile statetype lastState = Off;
skiley6 4:8442a7d55f23 44
skiley6 4:8442a7d55f23 45 enum editstate {editFront = 0, editRear, editAll};
skiley6 4:8442a7d55f23 46 volatile editstate edit = editAll;
skiley6 4:8442a7d55f23 47 volatile float cF = 0.5;
skiley6 4:8442a7d55f23 48 volatile float cR = 0.5;
skiley6 4:8442a7d55f23 49
cheryldocherty 0:04fef978a0ab 50
cheryldocherty 0:04fef978a0ab 51 // FUNCTION DECLARATIONS
skiley6 3:4b9d098dcb04 52 void blueRX()
cheryldocherty 0:04fef978a0ab 53 {
skiley6 3:4b9d098dcb04 54 char bnum=0;
skiley6 3:4b9d098dcb04 55 char bhit=0;
skiley6 3:4b9d098dcb04 56 while(1) {
skiley6 3:4b9d098dcb04 57 if (blue.readable()) { //if not ready with a character yield time slice!
skiley6 3:4b9d098dcb04 58 if (blue.getc()=='!') { //getc is one character only!
skiley6 3:4b9d098dcb04 59 //since GUI button sends all characters in string fast don't need to do
skiley6 3:4b9d098dcb04 60 //readable each time, but can if you wanted to
skiley6 3:4b9d098dcb04 61 if (blue.getc()=='B')
skiley6 3:4b9d098dcb04 62 { //button data
skiley6 3:4b9d098dcb04 63 bnum = blue.getc(); //button number
skiley6 3:4b9d098dcb04 64 bhit = blue.getc(); //save to check for '1' for hit only - ignored release is '0'
skiley6 3:4b9d098dcb04 65
skiley6 3:4b9d098dcb04 66 switch (bnum)
skiley6 3:4b9d098dcb04 67 {
skiley6 3:4b9d098dcb04 68 case '1': //number button 1 - DRS Enabled
skiley6 3:4b9d098dcb04 69 if (bhit=='1') {
skiley6 4:8442a7d55f23 70 if(servoState != DRS_Active)
skiley6 3:4b9d098dcb04 71 {
skiley6 4:8442a7d55f23 72 lastState = servoState;
skiley6 4:8442a7d55f23 73 servoState = DRS_Active;
skiley6 3:4b9d098dcb04 74 }
skiley6 3:4b9d098dcb04 75 else
skiley6 3:4b9d098dcb04 76 {
skiley6 4:8442a7d55f23 77 servoState = lastState;
skiley6 3:4b9d098dcb04 78 }
skiley6 4:8442a7d55f23 79 }
skiley6 3:4b9d098dcb04 80 break;
skiley6 3:4b9d098dcb04 81 case '2': //number button 2 - Active Aero Yaw Based
skiley6 3:4b9d098dcb04 82 if (bhit=='1') {
skiley6 3:4b9d098dcb04 83 if(servoState == Active_Yaw)
skiley6 3:4b9d098dcb04 84 {
skiley6 3:4b9d098dcb04 85 servoState = Off;
skiley6 3:4b9d098dcb04 86 }
skiley6 3:4b9d098dcb04 87 else
skiley6 3:4b9d098dcb04 88 {
skiley6 3:4b9d098dcb04 89 servoState = Active_Yaw;
skiley6 3:4b9d098dcb04 90 }
skiley6 4:8442a7d55f23 91 }
skiley6 3:4b9d098dcb04 92 break;
skiley6 3:4b9d098dcb04 93 case '3': //number button 3 - Active Aero Roll Based
skiley6 3:4b9d098dcb04 94 if (bhit=='1') {
skiley6 3:4b9d098dcb04 95 if(servoState == Active_Roll)
skiley6 3:4b9d098dcb04 96 {
skiley6 3:4b9d098dcb04 97 servoState = Off;
skiley6 3:4b9d098dcb04 98 }
skiley6 3:4b9d098dcb04 99 else
skiley6 3:4b9d098dcb04 100 {
skiley6 3:4b9d098dcb04 101 servoState = Active_Roll;
skiley6 3:4b9d098dcb04 102 }
skiley6 3:4b9d098dcb04 103 }
skiley6 3:4b9d098dcb04 104 break;
skiley6 3:4b9d098dcb04 105 case '4': //number button 4 - Testing Flaps
skiley6 3:4b9d098dcb04 106 if (bhit=='1') {
skiley6 3:4b9d098dcb04 107 servoState = Testing;
skiley6 4:8442a7d55f23 108 }
skiley6 4:8442a7d55f23 109 break;
skiley6 4:8442a7d55f23 110 case '5': //button 5 up arrow
skiley6 4:8442a7d55f23 111 if (bhit=='1') {
skiley6 4:8442a7d55f23 112 if(edit == editFront)
skiley6 4:8442a7d55f23 113 {
skiley6 4:8442a7d55f23 114 cF += 0.05;
skiley6 4:8442a7d55f23 115 if(cF > 0.5){
skiley6 4:8442a7d55f23 116 cF = 0.5;
skiley6 4:8442a7d55f23 117 }
skiley6 4:8442a7d55f23 118 //Thread::wait(20);
skiley6 4:8442a7d55f23 119 }
skiley6 4:8442a7d55f23 120 else if(edit == editRear)
skiley6 4:8442a7d55f23 121 {
skiley6 4:8442a7d55f23 122 cR += 0.05;
skiley6 4:8442a7d55f23 123 if(cR > 0.5){
skiley6 4:8442a7d55f23 124 cR = 0.5;
skiley6 4:8442a7d55f23 125 }
skiley6 4:8442a7d55f23 126 //Thread::wait(20);
skiley6 4:8442a7d55f23 127 }
skiley6 4:8442a7d55f23 128 else{
skiley6 4:8442a7d55f23 129 cF += 0.05;
skiley6 4:8442a7d55f23 130 cR += 0.05;
skiley6 4:8442a7d55f23 131
skiley6 4:8442a7d55f23 132 if(cR > 0.5){
skiley6 4:8442a7d55f23 133 cR = 0.5;
skiley6 4:8442a7d55f23 134 }
skiley6 4:8442a7d55f23 135 if(cF > 0.5){
skiley6 4:8442a7d55f23 136 cF = 0.5;
skiley6 4:8442a7d55f23 137 }
skiley6 4:8442a7d55f23 138 //Thread::wait(20);
skiley6 4:8442a7d55f23 139 }
skiley6 4:8442a7d55f23 140 }
skiley6 4:8442a7d55f23 141 break;
skiley6 4:8442a7d55f23 142 case '6': //button 6 down arrow
skiley6 4:8442a7d55f23 143 if (bhit=='1') {
skiley6 4:8442a7d55f23 144 if(edit == editFront)
skiley6 4:8442a7d55f23 145 {
skiley6 4:8442a7d55f23 146 cF -= 0.05;
skiley6 4:8442a7d55f23 147 if(cF < 0){
skiley6 4:8442a7d55f23 148 cF = 0;
skiley6 4:8442a7d55f23 149 }
skiley6 4:8442a7d55f23 150 //Thread::wait(20);
skiley6 4:8442a7d55f23 151 }
skiley6 4:8442a7d55f23 152 else if(edit == editRear)
skiley6 4:8442a7d55f23 153 {
skiley6 4:8442a7d55f23 154 cR -= 0.05;
skiley6 4:8442a7d55f23 155 if(cR < 0){
skiley6 4:8442a7d55f23 156 cR = 0;
skiley6 4:8442a7d55f23 157 }
skiley6 4:8442a7d55f23 158 //Thread::wait(20);
skiley6 4:8442a7d55f23 159 }
skiley6 4:8442a7d55f23 160 else{
skiley6 4:8442a7d55f23 161 cF -= 0.05;
skiley6 4:8442a7d55f23 162 cR -= 0.05;
skiley6 4:8442a7d55f23 163
skiley6 4:8442a7d55f23 164 if(cR < 0){
skiley6 4:8442a7d55f23 165 cR = 0;
skiley6 4:8442a7d55f23 166 }
skiley6 4:8442a7d55f23 167 if(cF < 0){
skiley6 4:8442a7d55f23 168 cF = 0;
skiley6 4:8442a7d55f23 169 }
skiley6 4:8442a7d55f23 170 //Thread::wait(20);
skiley6 4:8442a7d55f23 171 }
skiley6 4:8442a7d55f23 172 }
skiley6 4:8442a7d55f23 173 break;
skiley6 4:8442a7d55f23 174 case '7': //button 7 left arrow
skiley6 4:8442a7d55f23 175 if (bhit=='1') {
skiley6 4:8442a7d55f23 176 servoState = Off;
skiley6 4:8442a7d55f23 177 edit = editFront;
skiley6 4:8442a7d55f23 178 }
skiley6 4:8442a7d55f23 179 else{
skiley6 4:8442a7d55f23 180 edit = editAll;
skiley6 4:8442a7d55f23 181 }
skiley6 4:8442a7d55f23 182 break;
skiley6 4:8442a7d55f23 183 case '8': //button 8 right arrow
skiley6 4:8442a7d55f23 184 if (bhit=='1') {
skiley6 4:8442a7d55f23 185 servoState = Off;
skiley6 4:8442a7d55f23 186 edit = editRear;
skiley6 4:8442a7d55f23 187 }
skiley6 4:8442a7d55f23 188 else{
skiley6 4:8442a7d55f23 189 edit = editAll;
skiley6 3:4b9d098dcb04 190 }
skiley6 3:4b9d098dcb04 191 break;
skiley6 3:4b9d098dcb04 192 default:
skiley6 4:8442a7d55f23 193 break;
skiley6 3:4b9d098dcb04 194 }
skiley6 4:8442a7d55f23 195
skiley6 3:4b9d098dcb04 196 }
cheryldocherty 0:04fef978a0ab 197 }
skiley6 3:4b9d098dcb04 198 } else Thread::yield();// give up rest of time slice when no characters to read
skiley6 3:4b9d098dcb04 199 }
cheryldocherty 0:04fef978a0ab 200 }
cheryldocherty 0:04fef978a0ab 201
skiley6 3:4b9d098dcb04 202 void blueTX()
skiley6 3:4b9d098dcb04 203 {
skiley6 4:8442a7d55f23 204 //float lastF = 0;
skiley6 4:8442a7d55f23 205 //float last R = 0;
skiley6 4:8442a7d55f23 206 //statetype lastState = Off;
skiley6 4:8442a7d55f23 207 //editState lastEdit = editALL;
skiley6 4:8442a7d55f23 208 //blue.printf("Mode: HOLD........||Front:%3.0f%%||........||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 209
skiley6 3:4b9d098dcb04 210 while(1)
skiley6 3:4b9d098dcb04 211 {
skiley6 4:8442a7d55f23 212 //Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll
skiley6 3:4b9d098dcb04 213 //Send data
skiley6 4:8442a7d55f23 214
skiley6 4:8442a7d55f23 215 switch (servoState)
skiley6 4:8442a7d55f23 216 {
skiley6 4:8442a7d55f23 217 case Off:
skiley6 4:8442a7d55f23 218 if(edit == editFront)
skiley6 4:8442a7d55f23 219 {
skiley6 4:8442a7d55f23 220 blue.printf("Mode: HOLD.........||Front:%3.0f%%||.........Rear:%3.0f%%\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 221 }
skiley6 4:8442a7d55f23 222 else if(edit == editRear)
skiley6 4:8442a7d55f23 223 {
skiley6 4:8442a7d55f23 224 blue.printf("Mode: HOLD.........Front:%3.0f%%.........||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 225 }
skiley6 4:8442a7d55f23 226 else
skiley6 4:8442a7d55f23 227 {
skiley6 4:8442a7d55f23 228 blue.printf("Mode: HOLD......||Front:%3.0f%%||......||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 229 }
skiley6 4:8442a7d55f23 230 break;
skiley6 4:8442a7d55f23 231 case Testing:
skiley6 4:8442a7d55f23 232
skiley6 4:8442a7d55f23 233 blue.printf("Mode: Testing Flaps......................................\n");
skiley6 4:8442a7d55f23 234 break;
skiley6 4:8442a7d55f23 235 case DRS_Active:
skiley6 4:8442a7d55f23 236 blue.printf("....................Mode: DRS ACTIVATED....................\n");
skiley6 4:8442a7d55f23 237 break;
skiley6 4:8442a7d55f23 238 case Active_Yaw:
skiley6 5:295fe3425a73 239 //blue.printf("Mode: ACTIVE-YAW....Yaw:%3.0f deg/s....", YawRate);
skiley6 5:295fe3425a73 240 blue.printf("Mode: ACTIVE-YAW....Yaw:%3.0f deg/s....T:%sF\n", YawRate, t.print());
skiley6 4:8442a7d55f23 241 break;
skiley6 4:8442a7d55f23 242 case Active_Roll:
skiley6 5:295fe3425a73 243 //blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....", roll);
skiley6 5:295fe3425a73 244 blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....T:%sF\n", roll, t.print());
skiley6 4:8442a7d55f23 245 break;
skiley6 4:8442a7d55f23 246 default:
skiley6 4:8442a7d55f23 247 break;
skiley6 3:4b9d098dcb04 248
skiley6 4:8442a7d55f23 249 }
skiley6 5:295fe3425a73 250
skiley6 5:295fe3425a73 251 //blue.printf("Temp: %sºF, Pressure: %sPa\r\n", t.print(),p.print());
skiley6 3:4b9d098dcb04 252 Thread::wait(200);
skiley6 3:4b9d098dcb04 253 }
skiley6 3:4b9d098dcb04 254 }
cheryldocherty 0:04fef978a0ab 255 // IMU - caluclate pitch and roll
skiley6 4:8442a7d55f23 256 float getRoll(float ax, float az)
cheryldocherty 0:04fef978a0ab 257 {
skiley6 4:8442a7d55f23 258 float roll_t = atan2(ax, az);
skiley6 4:8442a7d55f23 259
skiley6 4:8442a7d55f23 260 roll_t *= 180.0 / PI;
skiley6 4:8442a7d55f23 261
skiley6 4:8442a7d55f23 262 return roll_t;
cheryldocherty 0:04fef978a0ab 263 }
cheryldocherty 0:04fef978a0ab 264
skiley6 5:295fe3425a73 265
cheryldocherty 0:04fef978a0ab 266 // IMU - read and display magnetometer, gyroscope, acceleration values
skiley6 5:295fe3425a73 267 void getIMUData()
cheryldocherty 0:04fef978a0ab 268 {
skiley6 5:295fe3425a73 269
skiley6 3:4b9d098dcb04 270 while(1)
skiley6 3:4b9d098dcb04 271 {
skiley6 3:4b9d098dcb04 272 while(!IMU.gyroAvailable());
skiley6 3:4b9d098dcb04 273 IMU.readGyro();
skiley6 4:8442a7d55f23 274 IMU.readAccel();
skiley6 5:295fe3425a73 275
skiley6 3:4b9d098dcb04 276 YawRate = IMU.calcGyro(IMU.gz);
skiley6 4:8442a7d55f23 277 roll = getRoll(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.az));
skiley6 5:295fe3425a73 278
skiley6 5:295fe3425a73 279
skiley6 5:295fe3425a73 280 //sensor.readTemperature(&t);
skiley6 5:295fe3425a73 281 //sensor.readPressure(&p);
skiley6 5:295fe3425a73 282
skiley6 3:4b9d098dcb04 283 Thread::wait(50);
skiley6 3:4b9d098dcb04 284 }
cheryldocherty 0:04fef978a0ab 285 }
cheryldocherty 0:04fef978a0ab 286
skiley6 3:4b9d098dcb04 287 void setServos()
skiley6 3:4b9d098dcb04 288 {
cheryldocherty 0:04fef978a0ab 289
skiley6 3:4b9d098dcb04 290 uint32_t testSpeed = 20;
skiley6 3:4b9d098dcb04 291 float testPrec = 0.05;
skiley6 3:4b9d098dcb04 292
skiley6 3:4b9d098dcb04 293
skiley6 3:4b9d098dcb04 294 while(1)
skiley6 3:4b9d098dcb04 295 {
skiley6 3:4b9d098dcb04 296 switch(servoState)
skiley6 3:4b9d098dcb04 297 {
skiley6 3:4b9d098dcb04 298 case Off: //close all flaps
skiley6 4:8442a7d55f23 299 servoFR = cF;
skiley6 4:8442a7d55f23 300 servoFL = 1 - cF;
skiley6 4:8442a7d55f23 301 servoRR = cR;
skiley6 4:8442a7d55f23 302 servoRL = 1 - cR;
skiley6 3:4b9d098dcb04 303
skiley6 3:4b9d098dcb04 304 Thread::wait(250); //longer wait because of Off state
skiley6 3:4b9d098dcb04 305 break;
skiley6 3:4b9d098dcb04 306 case Testing:
skiley6 3:4b9d098dcb04 307
skiley6 3:4b9d098dcb04 308 servoFR = 0;
skiley6 3:4b9d098dcb04 309 servoFL = 1;
skiley6 3:4b9d098dcb04 310 servoRR = 0;
skiley6 3:4b9d098dcb04 311 servoRL = 1;
skiley6 3:4b9d098dcb04 312 Thread::wait(500);
skiley6 3:4b9d098dcb04 313
skiley6 3:4b9d098dcb04 314 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 315 servoFR = i;
skiley6 3:4b9d098dcb04 316 servoRR = i;
skiley6 3:4b9d098dcb04 317 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 318 }
skiley6 3:4b9d098dcb04 319 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 320 servoFR = i;
skiley6 3:4b9d098dcb04 321 servoRR = i;
skiley6 3:4b9d098dcb04 322 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 323 }
skiley6 3:4b9d098dcb04 324
skiley6 3:4b9d098dcb04 325 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 326 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 327 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 328 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 329 }
skiley6 3:4b9d098dcb04 330 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 331 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 332 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 333 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 334 }
skiley6 3:4b9d098dcb04 335
skiley6 3:4b9d098dcb04 336 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 337 servoFR = i;
skiley6 3:4b9d098dcb04 338 servoRR = i;
skiley6 3:4b9d098dcb04 339 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 340 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 341 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 342 }
skiley6 3:4b9d098dcb04 343 for(float i=.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 344 servoFR = i;
skiley6 3:4b9d098dcb04 345 servoRR = i;
skiley6 3:4b9d098dcb04 346 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 347 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 348 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 349 }
skiley6 3:4b9d098dcb04 350 servoState = Off;
skiley6 3:4b9d098dcb04 351 break;
skiley6 3:4b9d098dcb04 352 case DRS_Active:
skiley6 3:4b9d098dcb04 353
skiley6 3:4b9d098dcb04 354 servoFR = 0.25;
skiley6 3:4b9d098dcb04 355 servoFL = 1 - 0.25;
skiley6 3:4b9d098dcb04 356 servoRR = 0;
skiley6 3:4b9d098dcb04 357 servoRL = 1 - 0;;
skiley6 3:4b9d098dcb04 358 Thread::wait(250);
skiley6 3:4b9d098dcb04 359
skiley6 3:4b9d098dcb04 360 break;
skiley6 3:4b9d098dcb04 361
skiley6 3:4b9d098dcb04 362 case Active_Yaw:
skiley6 3:4b9d098dcb04 363 float tempFront = 0.0;
skiley6 3:4b9d098dcb04 364 float tempRear = 0.0;
skiley6 3:4b9d098dcb04 365 float newFront = 0.0;
skiley6 3:4b9d098dcb04 366 float newRear = 0.0;
skiley6 3:4b9d098dcb04 367
skiley6 3:4b9d098dcb04 368 tempFront = YawRate/200; //divide to get into 0 -> 0.5 range for front wings
skiley6 3:4b9d098dcb04 369 tempRear = YawRate/400; //divide to get into 0 -> 0.25 range for rear wings
skiley6 3:4b9d098dcb04 370
skiley6 3:4b9d098dcb04 371 if(YawRate >= 10) //if turning Left
skiley6 3:4b9d098dcb04 372 {
skiley6 3:4b9d098dcb04 373 newFront = 0.5 - tempFront; //open up right flaps
skiley6 3:4b9d098dcb04 374 newRear = 0.25 - tempRear;
skiley6 3:4b9d098dcb04 375
skiley6 3:4b9d098dcb04 376 if(newFront < 0)
skiley6 3:4b9d098dcb04 377 {
skiley6 3:4b9d098dcb04 378 servoFR = 0;
skiley6 3:4b9d098dcb04 379 servoRR = 0.25;
skiley6 3:4b9d098dcb04 380 }
skiley6 3:4b9d098dcb04 381 else
skiley6 3:4b9d098dcb04 382 {
skiley6 3:4b9d098dcb04 383 servoFR = newFront;
skiley6 3:4b9d098dcb04 384 servoRR = newRear;
skiley6 3:4b9d098dcb04 385 }
skiley6 3:4b9d098dcb04 386
skiley6 3:4b9d098dcb04 387 servoFL = 0.5; //keep left side closed
skiley6 3:4b9d098dcb04 388 servoRL = 0.5;
skiley6 3:4b9d098dcb04 389 }
skiley6 3:4b9d098dcb04 390 else if(YawRate <= -10) // if turning Right
skiley6 3:4b9d098dcb04 391 {
skiley6 3:4b9d098dcb04 392 newFront = 1 - (0.5 + tempFront); // open up left flaps
skiley6 4:8442a7d55f23 393 newRear = 1 - (0.25 + tempRear);
skiley6 3:4b9d098dcb04 394 if(newFront < 0)
skiley6 3:4b9d098dcb04 395 {
skiley6 3:4b9d098dcb04 396 servoFL = 1 - 0;
skiley6 3:4b9d098dcb04 397 servoRL = 1 - 0.25;
skiley6 3:4b9d098dcb04 398 }
skiley6 3:4b9d098dcb04 399 else
skiley6 3:4b9d098dcb04 400 {
skiley6 3:4b9d098dcb04 401 servoFL = newFront;
skiley6 3:4b9d098dcb04 402 servoRL = newRear;
skiley6 3:4b9d098dcb04 403 }
skiley6 3:4b9d098dcb04 404 servoFR = 0.5; // keep right side closed
skiley6 3:4b9d098dcb04 405 servoRR = 0.5;
skiley6 3:4b9d098dcb04 406 }
skiley6 3:4b9d098dcb04 407 else
skiley6 3:4b9d098dcb04 408 {
skiley6 3:4b9d098dcb04 409 servoFR = 0.5; //set all to closed
skiley6 3:4b9d098dcb04 410 servoFL = 0.5;
skiley6 3:4b9d098dcb04 411 servoRR = 0.5;
skiley6 3:4b9d098dcb04 412 servoRL = 0.5;
skiley6 3:4b9d098dcb04 413 }
skiley6 3:4b9d098dcb04 414 Thread::wait(25);
skiley6 3:4b9d098dcb04 415
skiley6 3:4b9d098dcb04 416 break;
skiley6 3:4b9d098dcb04 417 case Active_Roll:
skiley6 4:8442a7d55f23 418 tempFront = 0.0;
skiley6 4:8442a7d55f23 419 tempRear = 0.0;
skiley6 4:8442a7d55f23 420 newFront = 0.0;
skiley6 4:8442a7d55f23 421 newRear = 0.0;
skiley6 4:8442a7d55f23 422
skiley6 4:8442a7d55f23 423 tempFront = roll/40; //divide to get into 0 -> 0.5 range for front wings
skiley6 4:8442a7d55f23 424 tempRear = roll/80; //divide to get into 0 -> 0.25 range for rear wings
skiley6 4:8442a7d55f23 425
skiley6 4:8442a7d55f23 426 if(roll >= 1) //if rolling left
skiley6 4:8442a7d55f23 427 {
skiley6 4:8442a7d55f23 428 newFront = 0.5 - tempFront; // open up left flaps
skiley6 4:8442a7d55f23 429 newRear = 0.5 - tempRear;
skiley6 4:8442a7d55f23 430 if(newFront < 0)
skiley6 4:8442a7d55f23 431 {
skiley6 4:8442a7d55f23 432 servoFL = 1 - 0;
skiley6 4:8442a7d55f23 433 servoRL = 1 - 0.25;
skiley6 4:8442a7d55f23 434 }
skiley6 4:8442a7d55f23 435 else
skiley6 4:8442a7d55f23 436 {
skiley6 4:8442a7d55f23 437 servoFL = 1 - newFront;
skiley6 4:8442a7d55f23 438 servoRL = 1 - newRear;
skiley6 4:8442a7d55f23 439 }
skiley6 4:8442a7d55f23 440 servoFR = 0.5; // keep right side closed
skiley6 4:8442a7d55f23 441 servoRR = 0.5;
skiley6 4:8442a7d55f23 442 }
skiley6 4:8442a7d55f23 443 else if(roll <= -1) // if rolling right
skiley6 4:8442a7d55f23 444 {
skiley6 4:8442a7d55f23 445 newFront = 0.5 + tempFront; //open up right flaps
skiley6 4:8442a7d55f23 446 newRear = 0.5 + tempRear;
skiley6 4:8442a7d55f23 447
skiley6 4:8442a7d55f23 448 if(newFront < 0)
skiley6 4:8442a7d55f23 449 {
skiley6 4:8442a7d55f23 450 servoFR = 0;
skiley6 4:8442a7d55f23 451 servoRR = 0.25;
skiley6 4:8442a7d55f23 452 }
skiley6 4:8442a7d55f23 453 else
skiley6 4:8442a7d55f23 454 {
skiley6 4:8442a7d55f23 455 servoFR = newFront;
skiley6 4:8442a7d55f23 456 servoRR = newRear;
skiley6 4:8442a7d55f23 457 }
skiley6 4:8442a7d55f23 458
skiley6 4:8442a7d55f23 459 servoFL = 0.5; //keep left side closed
skiley6 4:8442a7d55f23 460 servoRL = 0.5;
skiley6 4:8442a7d55f23 461 }
skiley6 4:8442a7d55f23 462 else
skiley6 4:8442a7d55f23 463 {
skiley6 4:8442a7d55f23 464 servoFR = 0.5; //set all to closed
skiley6 4:8442a7d55f23 465 servoFL = 0.5;
skiley6 4:8442a7d55f23 466 servoRR = 0.5;
skiley6 4:8442a7d55f23 467 servoRL = 0.5;
skiley6 4:8442a7d55f23 468 }
skiley6 4:8442a7d55f23 469 Thread::wait(25);
skiley6 3:4b9d098dcb04 470 break;
skiley6 3:4b9d098dcb04 471 default:
skiley6 4:8442a7d55f23 472 break;
skiley6 3:4b9d098dcb04 473 }
cheryldocherty 0:04fef978a0ab 474 }
cheryldocherty 0:04fef978a0ab 475 }
cheryldocherty 0:04fef978a0ab 476
abir77935 1:8e8aac99a366 477
cheryldocherty 0:04fef978a0ab 478
cheryldocherty 0:04fef978a0ab 479 int main() {
skiley6 3:4b9d098dcb04 480
skiley6 5:295fe3425a73 481 //init temp sensor
skiley6 5:295fe3425a73 482 sensor.init();
skiley6 5:295fe3425a73 483 // Offsets for Dacula, GA
skiley6 5:295fe3425a73 484 sensor.setOffsetAltitude(83);
skiley6 5:295fe3425a73 485 sensor.setOffsetTemperature(20);
skiley6 5:295fe3425a73 486 sensor.setOffsetPressure(-32);
skiley6 5:295fe3425a73 487
skiley6 5:295fe3425a73 488 sensor.setModeStandby();
skiley6 5:295fe3425a73 489 sensor.setModeBarometer();
skiley6 5:295fe3425a73 490 sensor.setModeActive();
skiley6 5:295fe3425a73 491
cheryldocherty 0:04fef978a0ab 492 // initialise IMU
cheryldocherty 0:04fef978a0ab 493 IMU.begin();
cheryldocherty 0:04fef978a0ab 494 if (!IMU.begin()) {
cheryldocherty 0:04fef978a0ab 495 pc.printf("Failed to communicate with LSM9DS1.\n");
cheryldocherty 0:04fef978a0ab 496 }
skiley6 5:295fe3425a73 497
skiley6 5:295fe3425a73 498
skiley6 3:4b9d098dcb04 499 //IMU.calibrate(1);
skiley6 3:4b9d098dcb04 500 //IMU.calibrateMag(0);
cheryldocherty 0:04fef978a0ab 501
abir77935 1:8e8aac99a366 502 blue.baud(9600); //set baud rate for UART window
skiley6 3:4b9d098dcb04 503
skiley6 3:4b9d098dcb04 504 blueRXThread.start(blueRX);
skiley6 3:4b9d098dcb04 505 blueTXThread.start(blueTX);
skiley6 5:295fe3425a73 506 IMUThread.start(getIMUData);
skiley6 3:4b9d098dcb04 507 servoThread.start(setServos);
skiley6 3:4b9d098dcb04 508
cheryldocherty 0:04fef978a0ab 509 while(1) {
skiley6 5:295fe3425a73 510 sensor.readTemperature(&t);
skiley6 5:295fe3425a73 511 Thread::wait(1000);
cheryldocherty 0:04fef978a0ab 512 }
cheryldocherty 0:04fef978a0ab 513 }