Source code for Active Aerodynamics and Drag Reduction System
Dependencies: mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2
Revision 4:8442a7d55f23, committed 2020-04-23
- Comitter:
- skiley6
- Date:
- Thu Apr 23 20:05:58 2020 +0000
- Parent:
- 3:4b9d098dcb04
- Child:
- 5:295fe3425a73
- Commit message:
- 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\
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Apr 22 22:04:47 2020 +0000 +++ b/main.cpp Thu Apr 23 20:05:58 2020 +0000 @@ -24,14 +24,23 @@ //THREADS Thread blueRXThread; Thread blueTXThread; -Thread IMUThread; +Thread sensorThread; Thread servoThread; // VARIABLE DECLARATIONS volatile float YawRate; +volatile float roll; +volatile float midTemp; enum statetype {Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll}; volatile statetype servoState = Off; +volatile statetype lastState = Off; + +enum editstate {editFront = 0, editRear, editAll}; +volatile editstate edit = editAll; +volatile float cF = 0.5; +volatile float cR = 0.5; + // FUNCTION DECLARATIONS void blueRX() @@ -52,17 +61,16 @@ { case '1': //number button 1 - DRS Enabled if (bhit=='1') { - if(servoState == DRS_Active) + if(servoState != DRS_Active) { - servoState = Off; + lastState = servoState; + servoState = DRS_Active; } else { - servoState = DRS_Active; + servoState = lastState; } - } else { - - } + } break; case '2': //number button 2 - Active Aero Yaw Based if (bhit=='1') { @@ -74,9 +82,7 @@ { servoState = Active_Yaw; } - } else { - - } + } break; case '3': //number button 3 - Active Aero Roll Based if (bhit=='1') { @@ -88,19 +94,99 @@ { servoState = Active_Roll; } - } else { - } break; case '4': //number button 4 - Testing Flaps if (bhit=='1') { servoState = Testing; - } else { - + } + break; + case '5': //button 5 up arrow + if (bhit=='1') { + if(edit == editFront) + { + cF += 0.05; + if(cF > 0.5){ + cF = 0.5; + } + //Thread::wait(20); + } + else if(edit == editRear) + { + cR += 0.05; + if(cR > 0.5){ + cR = 0.5; + } + //Thread::wait(20); + } + else{ + cF += 0.05; + cR += 0.05; + + if(cR > 0.5){ + cR = 0.5; + } + if(cF > 0.5){ + cF = 0.5; + } + //Thread::wait(20); + } + } + break; + case '6': //button 6 down arrow + if (bhit=='1') { + if(edit == editFront) + { + cF -= 0.05; + if(cF < 0){ + cF = 0; + } + //Thread::wait(20); + } + else if(edit == editRear) + { + cR -= 0.05; + if(cR < 0){ + cR = 0; + } + //Thread::wait(20); + } + else{ + cF -= 0.05; + cR -= 0.05; + + if(cR < 0){ + cR = 0; + } + if(cF < 0){ + cF = 0; + } + //Thread::wait(20); + } + } + break; + case '7': //button 7 left arrow + if (bhit=='1') { + servoState = Off; + edit = editFront; + } + else{ + edit = editAll; + } + break; + case '8': //button 8 right arrow + if (bhit=='1') { + servoState = Off; + edit = editRear; + } + else{ + edit = editAll; } break; default: + break; } + } } } else Thread::yield();// give up rest of time slice when no characters to read @@ -109,53 +195,75 @@ void blueTX() { - //float value; + //float lastF = 0; + //float last R = 0; + //statetype lastState = Off; + //editState lastEdit = editALL; + //blue.printf("Mode: HOLD........||Front:%3.0f%%||........||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100)); + while(1) { - + //Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll //Send data - blue.printf("%f\n", YawRate); - //value+=1.0; - led1 = !led1; + + switch (servoState) + { + case Off: + if(edit == editFront) + { + blue.printf("Mode: HOLD.........||Front:%3.0f%%||.........Rear:%3.0f%%\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100)); + } + else if(edit == editRear) + { + blue.printf("Mode: HOLD.........Front:%3.0f%%.........||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100)); + } + else + { + blue.printf("Mode: HOLD......||Front:%3.0f%%||......||Rear:%3.0f%%||\n", ceil(cF/0.5 * 100), ceil(cR/0.5 * 100)); + } + break; + case Testing: + + blue.printf("Mode: Testing Flaps......................................\n"); + break; + case DRS_Active: + blue.printf("....................Mode: DRS ACTIVATED....................\n"); + break; + case Active_Yaw: + blue.printf("Mode: ACTIVE-YAW.......YawRate: %3.0f deg/s........\n", YawRate); + break; + case Active_Roll: + blue.printf("Mode: ACTIVE-ROLL..........roll: %3.0f deg...............\n", roll); + break; + default: + break; + } Thread::wait(200); } } // IMU - caluclate pitch and roll -void printAttitude(float ax, float ay, float az, float mx, float my, float mz) +float getRoll(float ax, float az) { - float roll = atan2(ay, az); - float pitch = atan2(-ax, sqrt(ay * ay + az * az)); - // touchy trig stuff to use arctan to get compass heading (scale is 0..360) - mx = -mx; - float heading; - if (my == 0.0) - heading = (mx < 0.0) ? 180.0 : 0.0; - else - heading = atan2(mx, my)*360.0/(2.0*PI); - //pc.printf("heading atan=%f \n\r",heading); - heading -= DECLINATION; //correct for geo location - if(heading>180.0) heading = heading - 360.0; - else if(heading<-180.0) heading = 360.0 + heading; - else if(heading<0.0) heading = 360.0 + heading; - // Convert everything from radians to degrees: - //heading *= 180.0 / PI; - pitch *= 180.0 / PI; - roll *= 180.0 / PI; - pc.printf("Pitch: %f, Roll: %f degress\n\r",pitch,roll); - pc.printf("Magnetic Heading: %f degress\n\r",heading); + float roll_t = atan2(ax, az); + + roll_t *= 180.0 / PI; + + return roll_t; } // IMU - read and display magnetometer, gyroscope, acceleration values -void getIMUData() +void getSensorData() { while(1) { while(!IMU.gyroAvailable()); IMU.readGyro(); - + IMU.readAccel(); + //IMU.readTemp(); YawRate = IMU.calcGyro(IMU.gz); - + roll = getRoll(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.az)); + //midTemp = (25.0 + IMU.temperature/16.0) * (9/5) + 32; //get into C then into F Thread::wait(50); } @@ -190,10 +298,10 @@ switch(servoState) { case Off: //close all flaps - servoFR = 0.5; - servoFL = 0.5; - servoRR = 0.5; - servoRL = 0.5; + servoFR = cF; + servoFL = 1 - cF; + servoRR = cR; + servoRL = 1 - cR; Thread::wait(250); //longer wait because of Off state break; @@ -284,7 +392,7 @@ else if(YawRate <= -10) // if turning Right { newFront = 1 - (0.5 + tempFront); // open up left flaps - newRear = 1 - (0.5 + tempRear); + newRear = 1 - (0.25 + tempRear); if(newFront < 0) { servoFL = 1 - 0; @@ -309,9 +417,61 @@ break; case Active_Roll: - + tempFront = 0.0; + tempRear = 0.0; + newFront = 0.0; + newRear = 0.0; + + tempFront = roll/40; //divide to get into 0 -> 0.5 range for front wings + tempRear = roll/80; //divide to get into 0 -> 0.25 range for rear wings + + if(roll >= 1) //if rolling left + { + newFront = 0.5 - tempFront; // open up left flaps + newRear = 0.5 - tempRear; + if(newFront < 0) + { + servoFL = 1 - 0; + servoRL = 1 - 0.25; + } + else + { + servoFL = 1 - newFront; + servoRL = 1 - newRear; + } + servoFR = 0.5; // keep right side closed + servoRR = 0.5; + } + else if(roll <= -1) // if rolling right + { + newFront = 0.5 + tempFront; //open up right flaps + newRear = 0.5 + tempRear; + + if(newFront < 0) + { + servoFR = 0; + servoRR = 0.25; + } + else + { + servoFR = newFront; + servoRR = newRear; + } + + servoFL = 0.5; //keep left side closed + servoRL = 0.5; + } + else + { + servoFR = 0.5; //set all to closed + servoFL = 0.5; + servoRR = 0.5; + servoRL = 0.5; + } + Thread::wait(25); break; default: + break; } } } @@ -332,10 +492,9 @@ blueRXThread.start(blueRX); blueTXThread.start(blueTX); - IMUThread.start(getIMUData); + sensorThread.start(getSensorData); servoThread.start(setServos); - while(1) { }