File content as of revision 6:38cc8e010406:

#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;
                                     }
                                }
                                else if(edit == editRear)
                                {
                                    cR += 0.05;
                                     if(cR > 0.5){
                                         cR = 0.5;
                                     }
                                }
                                else{
                                    cF += 0.05;
                                    cR += 0.05;
                                    
                                     if(cR > 0.5){
                                         cR = 0.5;
                                     }
                                     if(cF > 0.5){
                                         cF = 0.5;
                                     }
                                }
                            }
                            break;
                        case '6': //button 6 down arrow
                            if (bhit=='1') {
                                if(edit == editFront)
                                {
                                     cF -= 0.05;
                                     if(cF < 0){
                                         cF = 0;
                                     }
                                }
                                else if(edit == editRear)
                                {
                                    cR -= 0.05;
                                     if(cR < 0){
                                         cR = 0;
                                     }
                                }
                                else{
                                    cF -= 0.05;
                                    cR -= 0.05;
                                    
                                     if(cR < 0){
                                         cR = 0;
                                     }
                                     if(cF < 0){
                                         cF = 0;
                                     }
                                }
                            }
                            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()
{
    while(1)
    {
        //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....T:%sF\n", YawRate, t.print());
                break;
            case Active_Roll:
                blue.printf("Mode: ACTIVE-ROLL....Roll: %3.0f deg....T:%sF\n", roll, t.print());
                break;
            default:
                break;   
        } 
        
        Thread::wait(200);
    }
}

// Calculate roll
float getRoll(float ax, float az)
{
    float roll_t = atan2(ax, az);
 
    roll_t  *= 180.0 / PI;

    return roll_t;
}

// IMU - read IMU gyro and accel data for YawRate and roll
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));
        
        Thread::wait(50);
    }
} 

// calculate and set servo positions
void setServos()
{

    uint32_t testSpeed = 20;    // thread wait length for movement
    float testPrec = 0.05;      // amount of movement per iteration 
     
    while(1)
    {
        switch(servoState)
        {
            case Off:   //close all flaps to set angle
                servoFR = cF;
                servoFL = 1 - cF;
                servoRR = cR;
                servoRL = 1 - cR;
                
                Thread::wait(250);  //longer wait because only based on user input changes
                break;
            case Testing:   //TESTING Mode
                
                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:    //DRS ACTIVE Mode, opens flaps to pre determined angle 
                
                servoFR = 0.25;
                servoFL = 1 - 0.25;
                servoRR = 0;
                servoRL = 1 - 0;;
                Thread::wait(250);
                
                break;
                
            case Active_Yaw:    // ACTIVE YAW mode. automatically adjust flaps based on the speed at which the vehicle turns
                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:       // ACTIVE ROLL Mode. automatically set flaps based on how much the vehicle rolls 
                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");
    }

    // Can Calibrate IMU if issues arise. 
    //IMU.calibrate(1);
    //IMU.calibrateMag(0);
    
    blue.baud(9600); //set baud rate for UART window
    
    //START THREADS
    blueRXThread.start(blueRX);
    blueTXThread.start(blueTX);
    IMUThread.start(getIMUData);
    servoThread.start(setServos);
    
    while(1) {  
        sensor.readTemperature(&t);
        Thread::wait(1000);
    }
}