Source code for Active Aerodynamics and Drag Reduction System
Dependencies: mbed Servo mbed-rtos LSM9DS1_Library_cal MPL3115A2
Revision 3:4b9d098dcb04, committed 2020-04-22
- Comitter:
- skiley6
- Date:
- Wed Apr 22 22:04:47 2020 +0000
- Parent:
- 2:426f26e9801d
- Child:
- 4:8442a7d55f23
- Commit message:
- Basic Functionality of DRS, Active with Yaw, and Testing buttons work properly. Most functions and threads created.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Apr 21 07:11:23 2020 +0000 +++ b/main.cpp Wed Apr 22 22:04:47 2020 +0000 @@ -6,39 +6,121 @@ #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA (Used in IMU code) -// OBJECTS -Servo servoFrontLeft(p21); -Servo servoFrontRight(p22); -Servo servoBackLeft(p23); -Servo servoBackRight(p24); +// SETUP +Servo servoFR(p21); +Servo servoFL(p22); +Servo servoRR(p23); +Servo servoRL(p24); LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); -Serial blue(p28,p27); // bluetooth +RawSerial blue(p13,p14); // bluetooth +Serial pc(USBTX, USBRX); // tx, rx + //BusOut myled(LED1,LED2,LED3,LED4); //bluetooth debugging -Thread thread1; -Serial pc(USBTX, USBRX); // 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 char bluetoothRead = 0; -volatile float xAccel = 0; -volatile float yAccel = 0; -volatile float zAccel = 0; -volatile float tuning = 1; +volatile float YawRate; +enum statetype {Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll}; +volatile statetype servoState = Off; // FUNCTION DECLARATIONS -void bluetooth_recv() +void blueRX() { - bluetoothRead = blue.getc(); - /*if (blue.getc()=='!') { - if (blue.getc()=='B') { //button data - bluetoothRead = blue.getc(); //button number - if ((bluetoothRead>='1')&&(bluetoothRead<='4')) //is a number button 1..4 - myled[bluetoothRead-'1']=blue.getc()-'0'; //turn on/off that num LED + 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) + { + servoState = Off; + } + else + { + servoState = DRS_Active; + } + } else { + + } + break; + case '2': //number button 2 - Active Aero Yaw Based + if (bhit=='1') { + if(servoState == Active_Yaw) + { + servoState = Off; + } + else + { + servoState = Active_Yaw; + } + } else { + + } + break; + case '3': //number button 3 - Active Aero Roll Based + if (bhit=='1') { + if(servoState == Active_Roll) + { + servoState = Off; + } + else + { + servoState = Active_Roll; + } + } else { + + } + break; + case '4': //number button 4 - Testing Flaps + if (bhit=='1') { + servoState = Testing; + } else { + + } + break; + default: + } + } } - } */ + } else Thread::yield();// give up rest of time slice when no characters to read + } } +void blueTX() +{ + //float value; + while(1) + { + + //Send data + blue.printf("%f\n", YawRate); + //value+=1.0; + led1 = !led1; + + Thread::wait(200); + } +} // IMU - caluclate pitch and roll void printAttitude(float ax, float ay, float az, float mx, float my, float mz) { @@ -67,6 +149,17 @@ // IMU - read and display magnetometer, gyroscope, acceleration values void getIMUData() { + while(1) + { + while(!IMU.gyroAvailable()); + IMU.readGyro(); + + YawRate = IMU.calcGyro(IMU.gz); + + Thread::wait(50); + } + + /* while(!IMU.magAvailable(X_AXIS)); IMU.readMag(); while(!IMU.accelAvailable()); @@ -82,53 +175,168 @@ zAccel = IMU.calcAccel(IMU.az);//setting global variable for storage of z-Axis acceleration yAccel = IMU.calcAccel(IMU.ay); xAccel = IMU.calcAccel(IMU.ax); + */ } +void setServos() +{ -// THREADS -void thread1Name() { - while (true) { - // things - Thread::wait(100); + uint32_t testSpeed = 20; + float testPrec = 0.05; + + + while(1) + { + switch(servoState) + { + case Off: //close all flaps + servoFR = 0.5; + servoFL = 0.5; + servoRR = 0.5; + servoRL = 0.5; + + 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.5 + 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: + + break; + default: + } } } -//Prints the acceraltion data to the bluetooth UART window -void blue_rec() -{ - while(1){ - blue.putc(zAccel); - Thread::wait(1000); - } -} -/* -//take in data from UART window on bluetooth to adjust tuning variables -void blue_tuning() -{ - while(dev.readable()) { - tuning = dev.getc(); - } -} -*/ int main() { + // initialise IMU IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } - IMU.calibrate(1); - IMU.calibrateMag(0); - - // Start threads - thread1.start(thread1Name); + //IMU.calibrate(1); + //IMU.calibrateMag(0); blue.baud(9600); //set baud rate for UART window - blue.attach(&blue_rec, Serial::RxIrq); + + blueRXThread.start(blueRX); + blueTXThread.start(blueTX); + IMUThread.start(getIMUData); + servoThread.start(setServos); + while(1) { - // things - Thread::wait(1000); + } }