Source code for Active Aerodynamics and Drag Reduction System
Dependencies: mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2
main.cpp
- Committer:
- skiley6
- Date:
- 2020-04-24
- Revision:
- 5:295fe3425a73
- Parent:
- 4:8442a7d55f23
- Child:
- 6:38cc8e010406
File content as of revision 5:295fe3425a73:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "LSM9DS1.h" #include "MPL3115A2.h" #define PI 3.14159 // Used in IMU code #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA (Used in IMU code) // SETUP Servo servoFR(p21); Servo servoFL(p22); Servo servoRR(p23); Servo servoRL(p24); LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); RawSerial blue(p13,p14); // bluetooth Serial pc(USBTX, USBRX); // tx, rx I2C i2c(p28, p27); // sda, scl MPL3115A2 sensor(&i2c, &pc); //BusOut myled(LED1,LED2,LED3,LED4); //bluetooth debugging DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); //THREADS Thread blueRXThread; Thread blueTXThread; Thread IMUThread; Thread servoThread; // VARIABLE DECLARATIONS volatile float YawRate; volatile float roll; Temperature t; 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() { char bnum=0; char bhit=0; while(1) { if (blue.readable()) { //if not ready with a character yield time slice! if (blue.getc()=='!') { //getc is one character only! //since GUI button sends all characters in string fast don't need to do //readable each time, but can if you wanted to if (blue.getc()=='B') { //button data bnum = blue.getc(); //button number bhit = blue.getc(); //save to check for '1' for hit only - ignored release is '0' switch (bnum) { case '1': //number button 1 - DRS Enabled if (bhit=='1') { if(servoState != DRS_Active) { lastState = servoState; servoState = DRS_Active; } else { servoState = lastState; } } break; case '2': //number button 2 - Active Aero Yaw Based if (bhit=='1') { if(servoState == Active_Yaw) { servoState = Off; } else { servoState = Active_Yaw; } } break; case '3': //number button 3 - Active Aero Roll Based if (bhit=='1') { if(servoState == Active_Roll) { servoState = Off; } else { servoState = Active_Roll; } } break; case '4': //number button 4 - Testing Flaps if (bhit=='1') { servoState = Testing; } 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 } } void blueTX() { //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 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....Yaw:%3.0f deg/s....", YawRate); blue.printf("Mode: ACTIVE-YAW....Yaw:%3.0f deg/s....T:%sF\n", YawRate, t.print()); break; case Active_Roll: //blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....", roll); blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....T:%sF\n", roll, t.print()); break; default: break; } //blue.printf("Temp: %sºF, Pressure: %sPa\r\n", t.print(),p.print()); Thread::wait(200); } } // IMU - caluclate pitch and roll float getRoll(float ax, float az) { float roll_t = atan2(ax, az); roll_t *= 180.0 / PI; return roll_t; } // IMU - read and display magnetometer, gyroscope, acceleration values void getIMUData() { while(1) { while(!IMU.gyroAvailable()); IMU.readGyro(); IMU.readAccel(); YawRate = IMU.calcGyro(IMU.gz); roll = getRoll(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.az)); //sensor.readTemperature(&t); //sensor.readPressure(&p); Thread::wait(50); } } void setServos() { uint32_t testSpeed = 20; float testPrec = 0.05; while(1) { switch(servoState) { case Off: //close all flaps servoFR = cF; servoFL = 1 - cF; servoRR = cR; servoRL = 1 - cR; Thread::wait(250); //longer wait because of Off state break; case Testing: servoFR = 0; servoFL = 1; servoRR = 0; servoRL = 1; Thread::wait(500); for(float i=0; i<0.5; i+= testPrec) { servoFR = i; servoRR = i; Thread::wait(testSpeed); } for(float i=0.5; i>0; i-= testPrec) { servoFR = i; servoRR = i; Thread::wait(testSpeed); } for(float i=0; i<0.5; i+= testPrec) { servoFL = 1 - i; servoRL = 1 - i; Thread::wait(testSpeed); } for(float i=0.5; i>0; i-= testPrec) { servoFL = 1 - i; servoRL = 1 - i; Thread::wait(testSpeed); } for(float i=0; i<0.5; i+= testPrec) { servoFR = i; servoRR = i; servoFL = 1 - i; servoRL = 1 - i; Thread::wait(testSpeed); } for(float i=.5; i>0; i-= testPrec) { servoFR = i; servoRR = i; servoFL = 1 - i; servoRL = 1 - i; Thread::wait(testSpeed); } servoState = Off; break; case DRS_Active: servoFR = 0.25; servoFL = 1 - 0.25; servoRR = 0; servoRL = 1 - 0;; Thread::wait(250); break; case Active_Yaw: float tempFront = 0.0; float tempRear = 0.0; float newFront = 0.0; float newRear = 0.0; tempFront = YawRate/200; //divide to get into 0 -> 0.5 range for front wings tempRear = YawRate/400; //divide to get into 0 -> 0.25 range for rear wings if(YawRate >= 10) //if turning Left { newFront = 0.5 - tempFront; //open up right flaps newRear = 0.25 - 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 if(YawRate <= -10) // if turning Right { newFront = 1 - (0.5 + tempFront); // open up left flaps newRear = 1 - (0.25 + tempRear); if(newFront < 0) { servoFL = 1 - 0; servoRL = 1 - 0.25; } else { servoFL = newFront; servoRL = newRear; } servoFR = 0.5; // keep right side closed servoRR = 0.5; } else { servoFR = 0.5; //set all to closed servoFL = 0.5; servoRR = 0.5; servoRL = 0.5; } Thread::wait(25); 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; } } } int main() { //init temp sensor sensor.init(); // Offsets for Dacula, GA sensor.setOffsetAltitude(83); sensor.setOffsetTemperature(20); sensor.setOffsetPressure(-32); sensor.setModeStandby(); sensor.setModeBarometer(); sensor.setModeActive(); // initialise IMU IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } //IMU.calibrate(1); //IMU.calibrateMag(0); blue.baud(9600); //set baud rate for UART window blueRXThread.start(blueRX); blueTXThread.start(blueTX); IMUThread.start(getIMUData); servoThread.start(setServos); while(1) { sensor.readTemperature(&t); Thread::wait(1000); } }