Source code for Active Aerodynamics and Drag Reduction System
Dependencies: mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2
Diff: main.cpp
- Revision:
- 4:8442a7d55f23
- Parent:
- 3:4b9d098dcb04
- Child:
- 5:295fe3425a73
diff -r 4b9d098dcb04 -r 8442a7d55f23 main.cpp --- 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) { }