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@4:8442a7d55f23, 2020-04-23 (annotated)

Committer:
skiley6
Date:
Thu Apr 23 20:05:58 2020 +0000
Revision:
4:8442a7d55f23
Parent:
3:4b9d098dcb04
Child:
5:295fe3425a73
Implemented Active Roll and DRS that toggles back to previous state. Also added correct information to be relaying back to phone based on state. Added Off mode adjustments for specific hold positions\

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