Active Aerodynamics and Drag Reduction System
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
--- 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) {
}