Diff: main.cpp

Revision:

3:4b9d098dcb04

Parent:

2:426f26e9801d

Child:

4:8442a7d55f23

--- 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);
+        
     }
 }