File content as of revision 3:4b9d098dcb04:

#include "mbed.h"
#include "rtos.h"
#include "Servo.h"
#include "LSM9DS1.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

//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;

enum statetype {Off = 0, Testing, DRS_Active, Active_Yaw, Active_Roll};
volatile statetype servoState = Off;

// 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)
                                {
                                    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)
{
    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);
}

// 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());
    IMU.readAccel();
    while(!IMU.gyroAvailable());
    IMU.readGyro();
    pc.printf("        X axis    Y axis    Z axis\n\r");
    pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
    pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
    pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
    printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
                  
    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()
{

    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:  
        } 
    }
}



int main() {
    
    // 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) {
        
    }
}