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@6:38cc8e010406, 2020-04-24 (annotated)

Committer:
skiley6
Date:
Fri Apr 24 16:06:47 2020 +0000
Revision:
6:38cc8e010406
Parent:
5:295fe3425a73 
Removed useless comments and made sure it compiles. This is able to be turned in currently

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 }
skiley6 4:8442a7d55f23 119 else if(edit == editRear)
skiley6 4:8442a7d55f23 120 {
skiley6 4:8442a7d55f23 121 cR += 0.05;
skiley6 4:8442a7d55f23 122 if(cR > 0.5){
skiley6 4:8442a7d55f23 123 cR = 0.5;
skiley6 4:8442a7d55f23 124 }
skiley6 4:8442a7d55f23 125 }
skiley6 4:8442a7d55f23 126 else{
skiley6 4:8442a7d55f23 127 cF += 0.05;
skiley6 4:8442a7d55f23 128 cR += 0.05;
skiley6 4:8442a7d55f23 129
skiley6 4:8442a7d55f23 130 if(cR > 0.5){
skiley6 4:8442a7d55f23 131 cR = 0.5;
skiley6 4:8442a7d55f23 132 }
skiley6 4:8442a7d55f23 133 if(cF > 0.5){
skiley6 4:8442a7d55f23 134 cF = 0.5;
skiley6 4:8442a7d55f23 135 }
skiley6 4:8442a7d55f23 136 }
skiley6 4:8442a7d55f23 137 }
skiley6 4:8442a7d55f23 138 break;
skiley6 4:8442a7d55f23 139 case '6': //button 6 down arrow
skiley6 4:8442a7d55f23 140 if (bhit=='1') {
skiley6 4:8442a7d55f23 141 if(edit == editFront)
skiley6 4:8442a7d55f23 142 {
skiley6 4:8442a7d55f23 143 cF -= 0.05;
skiley6 4:8442a7d55f23 144 if(cF < 0){
skiley6 4:8442a7d55f23 145 cF = 0;
skiley6 4:8442a7d55f23 146 }
skiley6 4:8442a7d55f23 147 }
skiley6 4:8442a7d55f23 148 else if(edit == editRear)
skiley6 4:8442a7d55f23 149 {
skiley6 4:8442a7d55f23 150 cR -= 0.05;
skiley6 4:8442a7d55f23 151 if(cR < 0){
skiley6 4:8442a7d55f23 152 cR = 0;
skiley6 4:8442a7d55f23 153 }
skiley6 4:8442a7d55f23 154 }
skiley6 4:8442a7d55f23 155 else{
skiley6 4:8442a7d55f23 156 cF -= 0.05;
skiley6 4:8442a7d55f23 157 cR -= 0.05;
skiley6 4:8442a7d55f23 158
skiley6 4:8442a7d55f23 159 if(cR < 0){
skiley6 4:8442a7d55f23 160 cR = 0;
skiley6 4:8442a7d55f23 161 }
skiley6 4:8442a7d55f23 162 if(cF < 0){
skiley6 4:8442a7d55f23 163 cF = 0;
skiley6 4:8442a7d55f23 164 }
skiley6 4:8442a7d55f23 165 }
skiley6 4:8442a7d55f23 166 }
skiley6 4:8442a7d55f23 167 break;
skiley6 4:8442a7d55f23 168 case '7': //button 7 left arrow
skiley6 4:8442a7d55f23 169 if (bhit=='1') {
skiley6 4:8442a7d55f23 170 servoState = Off;
skiley6 4:8442a7d55f23 171 edit = editFront;
skiley6 4:8442a7d55f23 172 }
skiley6 4:8442a7d55f23 173 else{
skiley6 4:8442a7d55f23 174 edit = editAll;
skiley6 4:8442a7d55f23 175 }
skiley6 4:8442a7d55f23 176 break;
skiley6 4:8442a7d55f23 177 case '8': //button 8 right arrow
skiley6 4:8442a7d55f23 178 if (bhit=='1') {
skiley6 4:8442a7d55f23 179 servoState = Off;
skiley6 4:8442a7d55f23 180 edit = editRear;
skiley6 4:8442a7d55f23 181 }
skiley6 4:8442a7d55f23 182 else{
skiley6 4:8442a7d55f23 183 edit = editAll;
skiley6 3:4b9d098dcb04 184 }
skiley6 3:4b9d098dcb04 185 break;
skiley6 3:4b9d098dcb04 186 default:
skiley6 4:8442a7d55f23 187 break;
skiley6 3:4b9d098dcb04 188 }
skiley6 4:8442a7d55f23 189
skiley6 3:4b9d098dcb04 190 }
cheryldocherty 0:04fef978a0ab 191 }
skiley6 3:4b9d098dcb04 192 } else Thread::yield();// give up rest of time slice when no characters to read
skiley6 3:4b9d098dcb04 193 }
cheryldocherty 0:04fef978a0ab 194 }
cheryldocherty 0:04fef978a0ab 195
skiley6 3:4b9d098dcb04 196 void blueTX()
skiley6 3:4b9d098dcb04 197 {
skiley6 3:4b9d098dcb04 198 while(1)
skiley6 3:4b9d098dcb04 199 {
skiley6 3:4b9d098dcb04 200 //Send data
skiley6 4:8442a7d55f23 201 switch (servoState)
skiley6 4:8442a7d55f23 202 {
skiley6 4:8442a7d55f23 203 case Off:
skiley6 4:8442a7d55f23 204 if(edit == editFront)
skiley6 4:8442a7d55f23 205 {
skiley6 4:8442a7d55f23 206 blue.printf("Mode: HOLD.........||Front:%3.0f%%||.........Rear:%3.0f%%\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 207 }
skiley6 4:8442a7d55f23 208 else if(edit == editRear)
skiley6 4:8442a7d55f23 209 {
skiley6 4:8442a7d55f23 210 blue.printf("Mode: HOLD.........Front:%3.0f%%.........||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 211 }
skiley6 4:8442a7d55f23 212 else
skiley6 4:8442a7d55f23 213 {
skiley6 4:8442a7d55f23 214 blue.printf("Mode: HOLD......||Front:%3.0f%%||......||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100));
skiley6 4:8442a7d55f23 215 }
skiley6 4:8442a7d55f23 216 break;
skiley6 4:8442a7d55f23 217 case Testing:
skiley6 4:8442a7d55f23 218
skiley6 4:8442a7d55f23 219 blue.printf("Mode: Testing Flaps......................................\n");
skiley6 4:8442a7d55f23 220 break;
skiley6 4:8442a7d55f23 221 case DRS_Active:
skiley6 4:8442a7d55f23 222 blue.printf("....................Mode: DRS ACTIVATED....................\n");
skiley6 4:8442a7d55f23 223 break;
skiley6 4:8442a7d55f23 224 case Active_Yaw:
skiley6 5:295fe3425a73 225 blue.printf("Mode: ACTIVE-YAW....Yaw:%3.0f deg/s....T:%sF\n", YawRate, t.print());
skiley6 4:8442a7d55f23 226 break;
skiley6 4:8442a7d55f23 227 case Active_Roll:
skiley6 5:295fe3425a73 228 blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....T:%sF\n", roll, t.print());
skiley6 4:8442a7d55f23 229 break;
skiley6 4:8442a7d55f23 230 default:
skiley6 4:8442a7d55f23 231 break;
skiley6 4:8442a7d55f23 232 }
skiley6 5:295fe3425a73 233
skiley6 3:4b9d098dcb04 234 Thread::wait(200);
skiley6 3:4b9d098dcb04 235 }
skiley6 3:4b9d098dcb04 236 }
skiley6 6:38cc8e010406 237
skiley6 6:38cc8e010406 238 // Calculate roll
skiley6 4:8442a7d55f23 239 float getRoll(float ax, float az)
cheryldocherty 0:04fef978a0ab 240 {
skiley6 4:8442a7d55f23 241 float roll_t = atan2(ax, az);
skiley6 4:8442a7d55f23 242
skiley6 4:8442a7d55f23 243 roll_t *= 180.0 / PI;
skiley6 4:8442a7d55f23 244
skiley6 4:8442a7d55f23 245 return roll_t;
cheryldocherty 0:04fef978a0ab 246 }
cheryldocherty 0:04fef978a0ab 247
skiley6 6:38cc8e010406 248 // IMU - read IMU gyro and accel data for YawRate and roll
skiley6 5:295fe3425a73 249 void getIMUData()
cheryldocherty 0:04fef978a0ab 250 {
skiley6 3:4b9d098dcb04 251 while(1)
skiley6 3:4b9d098dcb04 252 {
skiley6 3:4b9d098dcb04 253 while(!IMU.gyroAvailable());
skiley6 3:4b9d098dcb04 254 IMU.readGyro();
skiley6 4:8442a7d55f23 255 IMU.readAccel();
skiley6 5:295fe3425a73 256
skiley6 3:4b9d098dcb04 257 YawRate = IMU.calcGyro(IMU.gz);
skiley6 4:8442a7d55f23 258 roll = getRoll(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.az));
skiley6 5:295fe3425a73 259
skiley6 3:4b9d098dcb04 260 Thread::wait(50);
skiley6 3:4b9d098dcb04 261 }
cheryldocherty 0:04fef978a0ab 262 }
cheryldocherty 0:04fef978a0ab 263
skiley6 6:38cc8e010406 264 // calculate and set servo positions
skiley6 3:4b9d098dcb04 265 void setServos()
skiley6 3:4b9d098dcb04 266 {
cheryldocherty 0:04fef978a0ab 267
skiley6 6:38cc8e010406 268 uint32_t testSpeed = 20; // thread wait length for movement
skiley6 6:38cc8e010406 269 float testPrec = 0.05; // amount of movement per iteration
skiley6 6:38cc8e010406 270
skiley6 3:4b9d098dcb04 271 while(1)
skiley6 3:4b9d098dcb04 272 {
skiley6 3:4b9d098dcb04 273 switch(servoState)
skiley6 3:4b9d098dcb04 274 {
skiley6 6:38cc8e010406 275 case Off: //close all flaps to set angle
skiley6 4:8442a7d55f23 276 servoFR = cF;
skiley6 4:8442a7d55f23 277 servoFL = 1 - cF;
skiley6 4:8442a7d55f23 278 servoRR = cR;
skiley6 4:8442a7d55f23 279 servoRL = 1 - cR;
skiley6 3:4b9d098dcb04 280
skiley6 6:38cc8e010406 281 Thread::wait(250); //longer wait because only based on user input changes
skiley6 3:4b9d098dcb04 282 break;
skiley6 6:38cc8e010406 283 case Testing: //TESTING Mode
skiley6 3:4b9d098dcb04 284
skiley6 3:4b9d098dcb04 285 servoFR = 0;
skiley6 3:4b9d098dcb04 286 servoFL = 1;
skiley6 3:4b9d098dcb04 287 servoRR = 0;
skiley6 3:4b9d098dcb04 288 servoRL = 1;
skiley6 3:4b9d098dcb04 289 Thread::wait(500);
skiley6 3:4b9d098dcb04 290
skiley6 3:4b9d098dcb04 291 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 292 servoFR = i;
skiley6 3:4b9d098dcb04 293 servoRR = i;
skiley6 3:4b9d098dcb04 294 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 295 }
skiley6 3:4b9d098dcb04 296 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 297 servoFR = i;
skiley6 3:4b9d098dcb04 298 servoRR = i;
skiley6 3:4b9d098dcb04 299 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 300 }
skiley6 3:4b9d098dcb04 301
skiley6 3:4b9d098dcb04 302 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 303 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 304 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 305 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 306 }
skiley6 3:4b9d098dcb04 307 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 308 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 309 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 310 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 311 }
skiley6 3:4b9d098dcb04 312
skiley6 3:4b9d098dcb04 313 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 314 servoFR = i;
skiley6 3:4b9d098dcb04 315 servoRR = i;
skiley6 3:4b9d098dcb04 316 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 317 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 318 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 319 }
skiley6 3:4b9d098dcb04 320 for(float i=.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 321 servoFR = i;
skiley6 3:4b9d098dcb04 322 servoRR = i;
skiley6 3:4b9d098dcb04 323 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 324 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 325 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 326 }
skiley6 3:4b9d098dcb04 327 servoState = Off;
skiley6 3:4b9d098dcb04 328 break;
skiley6 6:38cc8e010406 329 case DRS_Active: //DRS ACTIVE Mode, opens flaps to pre determined angle
skiley6 3:4b9d098dcb04 330
skiley6 3:4b9d098dcb04 331 servoFR = 0.25;
skiley6 3:4b9d098dcb04 332 servoFL = 1 - 0.25;
skiley6 3:4b9d098dcb04 333 servoRR = 0;
skiley6 3:4b9d098dcb04 334 servoRL = 1 - 0;;
skiley6 3:4b9d098dcb04 335 Thread::wait(250);
skiley6 3:4b9d098dcb04 336
skiley6 3:4b9d098dcb04 337 break;
skiley6 3:4b9d098dcb04 338
skiley6 6:38cc8e010406 339 case Active_Yaw: // ACTIVE YAW mode. automatically adjust flaps based on the speed at which the vehicle turns
skiley6 3:4b9d098dcb04 340 float tempFront = 0.0;
skiley6 3:4b9d098dcb04 341 float tempRear = 0.0;
skiley6 3:4b9d098dcb04 342 float newFront = 0.0;
skiley6 3:4b9d098dcb04 343 float newRear = 0.0;
skiley6 3:4b9d098dcb04 344
skiley6 3:4b9d098dcb04 345 tempFront = YawRate/200; //divide to get into 0 -> 0.5 range for front wings
skiley6 3:4b9d098dcb04 346 tempRear = YawRate/400; //divide to get into 0 -> 0.25 range for rear wings
skiley6 3:4b9d098dcb04 347
skiley6 3:4b9d098dcb04 348 if(YawRate >= 10) //if turning Left
skiley6 3:4b9d098dcb04 349 {
skiley6 3:4b9d098dcb04 350 newFront = 0.5 - tempFront; //open up right flaps
skiley6 3:4b9d098dcb04 351 newRear = 0.25 - tempRear;
skiley6 3:4b9d098dcb04 352
skiley6 3:4b9d098dcb04 353 if(newFront < 0)
skiley6 3:4b9d098dcb04 354 {
skiley6 3:4b9d098dcb04 355 servoFR = 0;
skiley6 3:4b9d098dcb04 356 servoRR = 0.25;
skiley6 3:4b9d098dcb04 357 }
skiley6 3:4b9d098dcb04 358 else
skiley6 3:4b9d098dcb04 359 {
skiley6 3:4b9d098dcb04 360 servoFR = newFront;
skiley6 3:4b9d098dcb04 361 servoRR = newRear;
skiley6 3:4b9d098dcb04 362 }
skiley6 3:4b9d098dcb04 363
skiley6 3:4b9d098dcb04 364 servoFL = 0.5; //keep left side closed
skiley6 3:4b9d098dcb04 365 servoRL = 0.5;
skiley6 3:4b9d098dcb04 366 }
skiley6 3:4b9d098dcb04 367 else if(YawRate <= -10) // if turning Right
skiley6 3:4b9d098dcb04 368 {
skiley6 3:4b9d098dcb04 369 newFront = 1 - (0.5 + tempFront); // open up left flaps
skiley6 4:8442a7d55f23 370 newRear = 1 - (0.25 + tempRear);
skiley6 3:4b9d098dcb04 371 if(newFront < 0)
skiley6 3:4b9d098dcb04 372 {
skiley6 3:4b9d098dcb04 373 servoFL = 1 - 0;
skiley6 3:4b9d098dcb04 374 servoRL = 1 - 0.25;
skiley6 3:4b9d098dcb04 375 }
skiley6 3:4b9d098dcb04 376 else
skiley6 3:4b9d098dcb04 377 {
skiley6 3:4b9d098dcb04 378 servoFL = newFront;
skiley6 3:4b9d098dcb04 379 servoRL = newRear;
skiley6 3:4b9d098dcb04 380 }
skiley6 3:4b9d098dcb04 381 servoFR = 0.5; // keep right side closed
skiley6 3:4b9d098dcb04 382 servoRR = 0.5;
skiley6 3:4b9d098dcb04 383 }
skiley6 3:4b9d098dcb04 384 else
skiley6 3:4b9d098dcb04 385 {
skiley6 3:4b9d098dcb04 386 servoFR = 0.5; //set all to closed
skiley6 3:4b9d098dcb04 387 servoFL = 0.5;
skiley6 3:4b9d098dcb04 388 servoRR = 0.5;
skiley6 3:4b9d098dcb04 389 servoRL = 0.5;
skiley6 3:4b9d098dcb04 390 }
skiley6 3:4b9d098dcb04 391 Thread::wait(25);
skiley6 3:4b9d098dcb04 392
skiley6 3:4b9d098dcb04 393 break;
skiley6 6:38cc8e010406 394 case Active_Roll: // ACTIVE ROLL Mode. automatically set flaps based on how much the vehicle rolls
skiley6 4:8442a7d55f23 395 tempFront = 0.0;
skiley6 4:8442a7d55f23 396 tempRear = 0.0;
skiley6 4:8442a7d55f23 397 newFront = 0.0;
skiley6 4:8442a7d55f23 398 newRear = 0.0;
skiley6 4:8442a7d55f23 399
skiley6 4:8442a7d55f23 400 tempFront = roll/40; //divide to get into 0 -> 0.5 range for front wings
skiley6 4:8442a7d55f23 401 tempRear = roll/80; //divide to get into 0 -> 0.25 range for rear wings
skiley6 4:8442a7d55f23 402
skiley6 4:8442a7d55f23 403 if(roll >= 1) //if rolling left
skiley6 4:8442a7d55f23 404 {
skiley6 4:8442a7d55f23 405 newFront = 0.5 - tempFront; // open up left flaps
skiley6 4:8442a7d55f23 406 newRear = 0.5 - tempRear;
skiley6 4:8442a7d55f23 407 if(newFront < 0)
skiley6 4:8442a7d55f23 408 {
skiley6 4:8442a7d55f23 409 servoFL = 1 - 0;
skiley6 4:8442a7d55f23 410 servoRL = 1 - 0.25;
skiley6 4:8442a7d55f23 411 }
skiley6 4:8442a7d55f23 412 else
skiley6 4:8442a7d55f23 413 {
skiley6 4:8442a7d55f23 414 servoFL = 1 - newFront;
skiley6 4:8442a7d55f23 415 servoRL = 1 - newRear;
skiley6 4:8442a7d55f23 416 }
skiley6 4:8442a7d55f23 417 servoFR = 0.5; // keep right side closed
skiley6 4:8442a7d55f23 418 servoRR = 0.5;
skiley6 4:8442a7d55f23 419 }
skiley6 4:8442a7d55f23 420 else if(roll <= -1) // if rolling right
skiley6 4:8442a7d55f23 421 {
skiley6 4:8442a7d55f23 422 newFront = 0.5 + tempFront; //open up right flaps
skiley6 4:8442a7d55f23 423 newRear = 0.5 + tempRear;
skiley6 4:8442a7d55f23 424
skiley6 4:8442a7d55f23 425 if(newFront < 0)
skiley6 4:8442a7d55f23 426 {
skiley6 4:8442a7d55f23 427 servoFR = 0;
skiley6 4:8442a7d55f23 428 servoRR = 0.25;
skiley6 4:8442a7d55f23 429 }
skiley6 4:8442a7d55f23 430 else
skiley6 4:8442a7d55f23 431 {
skiley6 4:8442a7d55f23 432 servoFR = newFront;
skiley6 4:8442a7d55f23 433 servoRR = newRear;
skiley6 4:8442a7d55f23 434 }
skiley6 4:8442a7d55f23 435
skiley6 4:8442a7d55f23 436 servoFL = 0.5; //keep left side closed
skiley6 4:8442a7d55f23 437 servoRL = 0.5;
skiley6 4:8442a7d55f23 438 }
skiley6 4:8442a7d55f23 439 else
skiley6 4:8442a7d55f23 440 {
skiley6 4:8442a7d55f23 441 servoFR = 0.5; //set all to closed
skiley6 4:8442a7d55f23 442 servoFL = 0.5;
skiley6 4:8442a7d55f23 443 servoRR = 0.5;
skiley6 4:8442a7d55f23 444 servoRL = 0.5;
skiley6 4:8442a7d55f23 445 }
skiley6 4:8442a7d55f23 446 Thread::wait(25);
skiley6 3:4b9d098dcb04 447 break;
skiley6 3:4b9d098dcb04 448 default:
skiley6 4:8442a7d55f23 449 break;
skiley6 3:4b9d098dcb04 450 }
cheryldocherty 0:04fef978a0ab 451 }
cheryldocherty 0:04fef978a0ab 452 }
cheryldocherty 0:04fef978a0ab 453
abir77935 1:8e8aac99a366 454
cheryldocherty 0:04fef978a0ab 455
cheryldocherty 0:04fef978a0ab 456 int main() {
skiley6 3:4b9d098dcb04 457
skiley6 5:295fe3425a73 458 //init temp sensor
skiley6 5:295fe3425a73 459 sensor.init();
skiley6 5:295fe3425a73 460 // Offsets for Dacula, GA
skiley6 5:295fe3425a73 461 sensor.setOffsetAltitude(83);
skiley6 5:295fe3425a73 462 sensor.setOffsetTemperature(20);
skiley6 5:295fe3425a73 463 sensor.setOffsetPressure(-32);
skiley6 5:295fe3425a73 464
skiley6 5:295fe3425a73 465 sensor.setModeStandby();
skiley6 5:295fe3425a73 466 sensor.setModeBarometer();
skiley6 5:295fe3425a73 467 sensor.setModeActive();
skiley6 5:295fe3425a73 468
cheryldocherty 0:04fef978a0ab 469 // initialise IMU
cheryldocherty 0:04fef978a0ab 470 IMU.begin();
cheryldocherty 0:04fef978a0ab 471 if (!IMU.begin()) {
cheryldocherty 0:04fef978a0ab 472 pc.printf("Failed to communicate with LSM9DS1.\n");
cheryldocherty 0:04fef978a0ab 473 }
skiley6 5:295fe3425a73 474
skiley6 6:38cc8e010406 475 // Can Calibrate IMU if issues arise.
skiley6 3:4b9d098dcb04 476 //IMU.calibrate(1);
skiley6 3:4b9d098dcb04 477 //IMU.calibrateMag(0);
cheryldocherty 0:04fef978a0ab 478
abir77935 1:8e8aac99a366 479 blue.baud(9600); //set baud rate for UART window
skiley6 3:4b9d098dcb04 480
skiley6 6:38cc8e010406 481 //START THREADS
skiley6 3:4b9d098dcb04 482 blueRXThread.start(blueRX);
skiley6 3:4b9d098dcb04 483 blueTXThread.start(blueTX);
skiley6 5:295fe3425a73 484 IMUThread.start(getIMUData);
skiley6 3:4b9d098dcb04 485 servoThread.start(setServos);
skiley6 3:4b9d098dcb04 486
skiley6 6:38cc8e010406 487 while(1) {
skiley6 5:295fe3425a73 488 sensor.readTemperature(&t);
skiley6 5:295fe3425a73 489 Thread::wait(1000);
cheryldocherty 0:04fef978a0ab 490 }
cheryldocherty 0:04fef978a0ab 491 }