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@3:4b9d098dcb04, 2020-04-22 (annotated)

Committer:
skiley6
Date:
Wed Apr 22 22:04:47 2020 +0000
Revision:
3:4b9d098dcb04
Parent:
2:426f26e9801d
Child:
4:8442a7d55f23
Basic Functionality of DRS, Active with Yaw, and Testing buttons work properly. Most functions and threads created.

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 3:4b9d098dcb04 27 Thread IMUThread;
skiley6 3:4b9d098dcb04 28 Thread servoThread;
cheryldocherty 0:04fef978a0ab 29
cheryldocherty 0:04fef978a0ab 30 // VARIABLE DECLARATIONS
skiley6 3:4b9d098dcb04 31 volatile float YawRate;
cheryldocherty 0:04fef978a0ab 32
skiley6 3:4b9d098dcb04 33 enum statetype {Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll};
skiley6 3:4b9d098dcb04 34 volatile statetype servoState = Off;
cheryldocherty 0:04fef978a0ab 35
cheryldocherty 0:04fef978a0ab 36 // FUNCTION DECLARATIONS
skiley6 3:4b9d098dcb04 37 void blueRX()
cheryldocherty 0:04fef978a0ab 38 {
skiley6 3:4b9d098dcb04 39 char bnum=0;
skiley6 3:4b9d098dcb04 40 char bhit=0;
skiley6 3:4b9d098dcb04 41 while(1) {
skiley6 3:4b9d098dcb04 42 if (blue.readable()) { //if not ready with a character yield time slice!
skiley6 3:4b9d098dcb04 43 if (blue.getc()=='!') { //getc is one character only!
skiley6 3:4b9d098dcb04 44 //since GUI button sends all characters in string fast don't need to do
skiley6 3:4b9d098dcb04 45 //readable each time, but can if you wanted to
skiley6 3:4b9d098dcb04 46 if (blue.getc()=='B')
skiley6 3:4b9d098dcb04 47 { //button data
skiley6 3:4b9d098dcb04 48 bnum = blue.getc(); //button number
skiley6 3:4b9d098dcb04 49 bhit = blue.getc(); //save to check for '1' for hit only - ignored release is '0'
skiley6 3:4b9d098dcb04 50
skiley6 3:4b9d098dcb04 51 switch (bnum)
skiley6 3:4b9d098dcb04 52 {
skiley6 3:4b9d098dcb04 53 case '1': //number button 1 - DRS Enabled
skiley6 3:4b9d098dcb04 54 if (bhit=='1') {
skiley6 3:4b9d098dcb04 55 if(servoState == DRS_Active)
skiley6 3:4b9d098dcb04 56 {
skiley6 3:4b9d098dcb04 57 servoState = Off;
skiley6 3:4b9d098dcb04 58 }
skiley6 3:4b9d098dcb04 59 else
skiley6 3:4b9d098dcb04 60 {
skiley6 3:4b9d098dcb04 61 servoState = DRS_Active;
skiley6 3:4b9d098dcb04 62 }
skiley6 3:4b9d098dcb04 63 } else {
skiley6 3:4b9d098dcb04 64
skiley6 3:4b9d098dcb04 65 }
skiley6 3:4b9d098dcb04 66 break;
skiley6 3:4b9d098dcb04 67 case '2': //number button 2 - Active Aero Yaw Based
skiley6 3:4b9d098dcb04 68 if (bhit=='1') {
skiley6 3:4b9d098dcb04 69 if(servoState == Active_Yaw)
skiley6 3:4b9d098dcb04 70 {
skiley6 3:4b9d098dcb04 71 servoState = Off;
skiley6 3:4b9d098dcb04 72 }
skiley6 3:4b9d098dcb04 73 else
skiley6 3:4b9d098dcb04 74 {
skiley6 3:4b9d098dcb04 75 servoState = Active_Yaw;
skiley6 3:4b9d098dcb04 76 }
skiley6 3:4b9d098dcb04 77 } else {
skiley6 3:4b9d098dcb04 78
skiley6 3:4b9d098dcb04 79 }
skiley6 3:4b9d098dcb04 80 break;
skiley6 3:4b9d098dcb04 81 case '3': //number button 3 - Active Aero Roll Based
skiley6 3:4b9d098dcb04 82 if (bhit=='1') {
skiley6 3:4b9d098dcb04 83 if(servoState == Active_Roll)
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_Roll;
skiley6 3:4b9d098dcb04 90 }
skiley6 3:4b9d098dcb04 91 } else {
skiley6 3:4b9d098dcb04 92
skiley6 3:4b9d098dcb04 93 }
skiley6 3:4b9d098dcb04 94 break;
skiley6 3:4b9d098dcb04 95 case '4': //number button 4 - Testing Flaps
skiley6 3:4b9d098dcb04 96 if (bhit=='1') {
skiley6 3:4b9d098dcb04 97 servoState = Testing;
skiley6 3:4b9d098dcb04 98 } else {
skiley6 3:4b9d098dcb04 99
skiley6 3:4b9d098dcb04 100 }
skiley6 3:4b9d098dcb04 101 break;
skiley6 3:4b9d098dcb04 102 default:
skiley6 3:4b9d098dcb04 103 }
skiley6 3:4b9d098dcb04 104 }
cheryldocherty 0:04fef978a0ab 105 }
skiley6 3:4b9d098dcb04 106 } else Thread::yield();// give up rest of time slice when no characters to read
skiley6 3:4b9d098dcb04 107 }
cheryldocherty 0:04fef978a0ab 108 }
cheryldocherty 0:04fef978a0ab 109
skiley6 3:4b9d098dcb04 110 void blueTX()
skiley6 3:4b9d098dcb04 111 {
skiley6 3:4b9d098dcb04 112 //float value;
skiley6 3:4b9d098dcb04 113 while(1)
skiley6 3:4b9d098dcb04 114 {
skiley6 3:4b9d098dcb04 115
skiley6 3:4b9d098dcb04 116 //Send data
skiley6 3:4b9d098dcb04 117 blue.printf("%f\n", YawRate);
skiley6 3:4b9d098dcb04 118 //value+=1.0;
skiley6 3:4b9d098dcb04 119 led1 = !led1;
skiley6 3:4b9d098dcb04 120
skiley6 3:4b9d098dcb04 121 Thread::wait(200);
skiley6 3:4b9d098dcb04 122 }
skiley6 3:4b9d098dcb04 123 }
cheryldocherty 0:04fef978a0ab 124 // IMU - caluclate pitch and roll
cheryldocherty 0:04fef978a0ab 125 void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
cheryldocherty 0:04fef978a0ab 126 {
cheryldocherty 0:04fef978a0ab 127 float roll = atan2(ay, az);
cheryldocherty 0:04fef978a0ab 128 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
cheryldocherty 0:04fef978a0ab 129 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
cheryldocherty 0:04fef978a0ab 130 mx = -mx;
cheryldocherty 0:04fef978a0ab 131 float heading;
cheryldocherty 0:04fef978a0ab 132 if (my == 0.0)
cheryldocherty 0:04fef978a0ab 133 heading = (mx < 0.0) ? 180.0 : 0.0;
cheryldocherty 0:04fef978a0ab 134 else
cheryldocherty 0:04fef978a0ab 135 heading = atan2(mx, my)*360.0/(2.0*PI);
cheryldocherty 0:04fef978a0ab 136 //pc.printf("heading atan=%f \n\r",heading);
cheryldocherty 0:04fef978a0ab 137 heading -= DECLINATION; //correct for geo location
cheryldocherty 0:04fef978a0ab 138 if(heading>180.0) heading = heading - 360.0;
cheryldocherty 0:04fef978a0ab 139 else if(heading<-180.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 140 else if(heading<0.0) heading = 360.0 + heading;
cheryldocherty 0:04fef978a0ab 141 // Convert everything from radians to degrees:
cheryldocherty 0:04fef978a0ab 142 //heading *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 143 pitch *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 144 roll *= 180.0 / PI;
cheryldocherty 0:04fef978a0ab 145 pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll);
cheryldocherty 0:04fef978a0ab 146 pc.printf("Magnetic Heading: %f degress\n\r",heading);
cheryldocherty 0:04fef978a0ab 147 }
cheryldocherty 0:04fef978a0ab 148
cheryldocherty 0:04fef978a0ab 149 // IMU - read and display magnetometer, gyroscope, acceleration values
cheryldocherty 0:04fef978a0ab 150 void getIMUData()
cheryldocherty 0:04fef978a0ab 151 {
skiley6 3:4b9d098dcb04 152 while(1)
skiley6 3:4b9d098dcb04 153 {
skiley6 3:4b9d098dcb04 154 while(!IMU.gyroAvailable());
skiley6 3:4b9d098dcb04 155 IMU.readGyro();
skiley6 3:4b9d098dcb04 156
skiley6 3:4b9d098dcb04 157 YawRate = IMU.calcGyro(IMU.gz);
skiley6 3:4b9d098dcb04 158
skiley6 3:4b9d098dcb04 159 Thread::wait(50);
skiley6 3:4b9d098dcb04 160 }
skiley6 3:4b9d098dcb04 161
skiley6 3:4b9d098dcb04 162 /*
cheryldocherty 0:04fef978a0ab 163 while(!IMU.magAvailable(X_AXIS));
cheryldocherty 0:04fef978a0ab 164 IMU.readMag();
cheryldocherty 0:04fef978a0ab 165 while(!IMU.accelAvailable());
cheryldocherty 0:04fef978a0ab 166 IMU.readAccel();
cheryldocherty 0:04fef978a0ab 167 while(!IMU.gyroAvailable());
cheryldocherty 0:04fef978a0ab 168 IMU.readGyro();
cheryldocherty 0:04fef978a0ab 169 pc.printf(" X axis Y axis Z axis\n\r");
cheryldocherty 0:04fef978a0ab 170 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 171 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 172 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 173 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 174
abir77935 2:426f26e9801d 175 zAccel = IMU.calcAccel(IMU.az);//setting global variable for storage of z-Axis acceleration
abir77935 2:426f26e9801d 176 yAccel = IMU.calcAccel(IMU.ay);
abir77935 2:426f26e9801d 177 xAccel = IMU.calcAccel(IMU.ax);
skiley6 3:4b9d098dcb04 178 */
cheryldocherty 0:04fef978a0ab 179 }
cheryldocherty 0:04fef978a0ab 180
skiley6 3:4b9d098dcb04 181 void setServos()
skiley6 3:4b9d098dcb04 182 {
cheryldocherty 0:04fef978a0ab 183
skiley6 3:4b9d098dcb04 184 uint32_t testSpeed = 20;
skiley6 3:4b9d098dcb04 185 float testPrec = 0.05;
skiley6 3:4b9d098dcb04 186
skiley6 3:4b9d098dcb04 187
skiley6 3:4b9d098dcb04 188 while(1)
skiley6 3:4b9d098dcb04 189 {
skiley6 3:4b9d098dcb04 190 switch(servoState)
skiley6 3:4b9d098dcb04 191 {
skiley6 3:4b9d098dcb04 192 case Off: //close all flaps
skiley6 3:4b9d098dcb04 193 servoFR = 0.5;
skiley6 3:4b9d098dcb04 194 servoFL = 0.5;
skiley6 3:4b9d098dcb04 195 servoRR = 0.5;
skiley6 3:4b9d098dcb04 196 servoRL = 0.5;
skiley6 3:4b9d098dcb04 197
skiley6 3:4b9d098dcb04 198 Thread::wait(250); //longer wait because of Off state
skiley6 3:4b9d098dcb04 199 break;
skiley6 3:4b9d098dcb04 200 case Testing:
skiley6 3:4b9d098dcb04 201
skiley6 3:4b9d098dcb04 202 servoFR = 0;
skiley6 3:4b9d098dcb04 203 servoFL = 1;
skiley6 3:4b9d098dcb04 204 servoRR = 0;
skiley6 3:4b9d098dcb04 205 servoRL = 1;
skiley6 3:4b9d098dcb04 206 Thread::wait(500);
skiley6 3:4b9d098dcb04 207
skiley6 3:4b9d098dcb04 208 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 209 servoFR = i;
skiley6 3:4b9d098dcb04 210 servoRR = i;
skiley6 3:4b9d098dcb04 211 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 212 }
skiley6 3:4b9d098dcb04 213 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 214 servoFR = i;
skiley6 3:4b9d098dcb04 215 servoRR = i;
skiley6 3:4b9d098dcb04 216 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 217 }
skiley6 3:4b9d098dcb04 218
skiley6 3:4b9d098dcb04 219 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 220 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 221 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 222 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 223 }
skiley6 3:4b9d098dcb04 224 for(float i=0.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 225 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 226 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 227 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 228 }
skiley6 3:4b9d098dcb04 229
skiley6 3:4b9d098dcb04 230 for(float i=0; i<0.5; i+= testPrec) {
skiley6 3:4b9d098dcb04 231 servoFR = i;
skiley6 3:4b9d098dcb04 232 servoRR = i;
skiley6 3:4b9d098dcb04 233 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 234 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 235 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 236 }
skiley6 3:4b9d098dcb04 237 for(float i=.5; i>0; i-= testPrec) {
skiley6 3:4b9d098dcb04 238 servoFR = i;
skiley6 3:4b9d098dcb04 239 servoRR = i;
skiley6 3:4b9d098dcb04 240 servoFL = 1 - i;
skiley6 3:4b9d098dcb04 241 servoRL = 1 - i;
skiley6 3:4b9d098dcb04 242 Thread::wait(testSpeed);
skiley6 3:4b9d098dcb04 243 }
skiley6 3:4b9d098dcb04 244 servoState = Off;
skiley6 3:4b9d098dcb04 245 break;
skiley6 3:4b9d098dcb04 246 case DRS_Active:
skiley6 3:4b9d098dcb04 247
skiley6 3:4b9d098dcb04 248 servoFR = 0.25;
skiley6 3:4b9d098dcb04 249 servoFL = 1 - 0.25;
skiley6 3:4b9d098dcb04 250 servoRR = 0;
skiley6 3:4b9d098dcb04 251 servoRL = 1 - 0;;
skiley6 3:4b9d098dcb04 252 Thread::wait(250);
skiley6 3:4b9d098dcb04 253
skiley6 3:4b9d098dcb04 254 break;
skiley6 3:4b9d098dcb04 255
skiley6 3:4b9d098dcb04 256 case Active_Yaw:
skiley6 3:4b9d098dcb04 257 float tempFront = 0.0;
skiley6 3:4b9d098dcb04 258 float tempRear = 0.0;
skiley6 3:4b9d098dcb04 259 float newFront = 0.0;
skiley6 3:4b9d098dcb04 260 float newRear = 0.0;
skiley6 3:4b9d098dcb04 261
skiley6 3:4b9d098dcb04 262 tempFront = YawRate/200; //divide to get into 0 -> 0.5 range for front wings
skiley6 3:4b9d098dcb04 263 tempRear = YawRate/400; //divide to get into 0 -> 0.25 range for rear wings
skiley6 3:4b9d098dcb04 264
skiley6 3:4b9d098dcb04 265 if(YawRate >= 10) //if turning Left
skiley6 3:4b9d098dcb04 266 {
skiley6 3:4b9d098dcb04 267 newFront = 0.5 - tempFront; //open up right flaps
skiley6 3:4b9d098dcb04 268 newRear = 0.25 - tempRear;
skiley6 3:4b9d098dcb04 269
skiley6 3:4b9d098dcb04 270 if(newFront < 0)
skiley6 3:4b9d098dcb04 271 {
skiley6 3:4b9d098dcb04 272 servoFR = 0;
skiley6 3:4b9d098dcb04 273 servoRR = 0.25;
skiley6 3:4b9d098dcb04 274 }
skiley6 3:4b9d098dcb04 275 else
skiley6 3:4b9d098dcb04 276 {
skiley6 3:4b9d098dcb04 277 servoFR = newFront;
skiley6 3:4b9d098dcb04 278 servoRR = newRear;
skiley6 3:4b9d098dcb04 279 }
skiley6 3:4b9d098dcb04 280
skiley6 3:4b9d098dcb04 281 servoFL = 0.5; //keep left side closed
skiley6 3:4b9d098dcb04 282 servoRL = 0.5;
skiley6 3:4b9d098dcb04 283 }
skiley6 3:4b9d098dcb04 284 else if(YawRate <= -10) // if turning Right
skiley6 3:4b9d098dcb04 285 {
skiley6 3:4b9d098dcb04 286 newFront = 1 - (0.5 + tempFront); // open up left flaps
skiley6 3:4b9d098dcb04 287 newRear = 1 - (0.5 + tempRear);
skiley6 3:4b9d098dcb04 288 if(newFront < 0)
skiley6 3:4b9d098dcb04 289 {
skiley6 3:4b9d098dcb04 290 servoFL = 1 - 0;
skiley6 3:4b9d098dcb04 291 servoRL = 1 - 0.25;
skiley6 3:4b9d098dcb04 292 }
skiley6 3:4b9d098dcb04 293 else
skiley6 3:4b9d098dcb04 294 {
skiley6 3:4b9d098dcb04 295 servoFL = newFront;
skiley6 3:4b9d098dcb04 296 servoRL = newRear;
skiley6 3:4b9d098dcb04 297 }
skiley6 3:4b9d098dcb04 298 servoFR = 0.5; // keep right side closed
skiley6 3:4b9d098dcb04 299 servoRR = 0.5;
skiley6 3:4b9d098dcb04 300 }
skiley6 3:4b9d098dcb04 301 else
skiley6 3:4b9d098dcb04 302 {
skiley6 3:4b9d098dcb04 303 servoFR = 0.5; //set all to closed
skiley6 3:4b9d098dcb04 304 servoFL = 0.5;
skiley6 3:4b9d098dcb04 305 servoRR = 0.5;
skiley6 3:4b9d098dcb04 306 servoRL = 0.5;
skiley6 3:4b9d098dcb04 307 }
skiley6 3:4b9d098dcb04 308 Thread::wait(25);
skiley6 3:4b9d098dcb04 309
skiley6 3:4b9d098dcb04 310 break;
skiley6 3:4b9d098dcb04 311 case Active_Roll:
skiley6 3:4b9d098dcb04 312
skiley6 3:4b9d098dcb04 313 break;
skiley6 3:4b9d098dcb04 314 default:
skiley6 3:4b9d098dcb04 315 }
cheryldocherty 0:04fef978a0ab 316 }
cheryldocherty 0:04fef978a0ab 317 }
cheryldocherty 0:04fef978a0ab 318
abir77935 1:8e8aac99a366 319
cheryldocherty 0:04fef978a0ab 320
cheryldocherty 0:04fef978a0ab 321 int main() {
skiley6 3:4b9d098dcb04 322
cheryldocherty 0:04fef978a0ab 323 // initialise IMU
cheryldocherty 0:04fef978a0ab 324 IMU.begin();
cheryldocherty 0:04fef978a0ab 325 if (!IMU.begin()) {
cheryldocherty 0:04fef978a0ab 326 pc.printf("Failed to communicate with LSM9DS1.\n");
cheryldocherty 0:04fef978a0ab 327 }
skiley6 3:4b9d098dcb04 328 //IMU.calibrate(1);
skiley6 3:4b9d098dcb04 329 //IMU.calibrateMag(0);
cheryldocherty 0:04fef978a0ab 330
abir77935 1:8e8aac99a366 331 blue.baud(9600); //set baud rate for UART window
skiley6 3:4b9d098dcb04 332
skiley6 3:4b9d098dcb04 333 blueRXThread.start(blueRX);
skiley6 3:4b9d098dcb04 334 blueTXThread.start(blueTX);
skiley6 3:4b9d098dcb04 335 IMUThread.start(getIMUData);
skiley6 3:4b9d098dcb04 336 servoThread.start(setServos);
skiley6 3:4b9d098dcb04 337
abir77935 1:8e8aac99a366 338
cheryldocherty 0:04fef978a0ab 339 while(1) {
skiley6 3:4b9d098dcb04 340
cheryldocherty 0:04fef978a0ab 341 }
cheryldocherty 0:04fef978a0ab 342 }