File content as of revision 0:d18628ed3ec3:

#include "mbed.h"
#include "Servo.h"
#include "PwmIn.h"

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////        setup        //////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Initialize left and right servos
Servo back_servo(p23);
Servo front_servo(p24);

//E-Stop Button
DigitalIn Ctrl(p8);

//Pins for Thruster Power
Servo front_thrust(p25);
Servo back_thrust(p26);

//Initialize receiver pins
PwmIn thro(p17);
PwmIn elev(p15);
PwmIn gear(p13);
PwmIn aile(p14);
PwmIn rudd(p16);
PwmIn aux(p18);

Serial pc(USBTX, USBRX);
Serial ledControl(p28, p27);

Timer nickTime;
Timer ledTime;

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////  global variables   //////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Set all PWM values to a defaalt of 0
float throttle_output = 0.0;
float elevation_output = 0.0;
float ESTOP_output = 0.0;
float rudder_output = 0.0;
float aileron_output = 0.0;
float aux_output = 0.0;

//Variables for Serial Communcation with Labview
volatile bool newData = false;
volatile float inputs[4];

//light tower
float ledColor = 0;
float ledBright = 0;

//union for things in the light tower
union {
  float f;
  char  bytes[4];
} float_union;

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////     Functions      ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////

//Function changes 0 t 1 input into 0.08 to 0.92
float RangeChange(float x)
{
    float y;
    y = ((x * 0.84) + 0.08);
    return y;
}


//Function Reads Serial com
void onSerialRx()
{
    static char serialInBuffer[100]; 
    static int serialCount = 0;
 
    while (pc.readable())// in case two bytes are ready
    {
        char byteIn = pc.getc();//Read serial message
        
        if (byteIn == 0x65)// if an end of line is found
        { 
            serialInBuffer[serialCount] == 0; // null terminate the input                
            float w,x,y,z;
            if (sscanf(serialInBuffer,"%f,%f,%f,%fe",&w,&x,&y,&z) == 4)// managed to read all 4 values
            { 
                inputs[0] = w;
                inputs[1] = x;
                inputs[2] = y;
                inputs[3] = z;
                newData = true;
            }
            serialCount = 0; // reset the buffer
        } 
        else
        {
            serialInBuffer[serialCount] = byteIn;// store the character
            if (serialCount<100)
            {
                serialCount++;// increase the counter.
            }
        }
    }
}


void Calibrate()
{
    //Calibration Sequence
    back_servo = 0.0;
    front_servo = 0.0;
    back_thrust = 0.0;
    front_thrust = 0.0;
    wait(0.1); //ESC detects signal
    //Required ESC Calibration/Arming sequence  
    //sends longest and shortest PWM pulse to learn and arm at power on
    back_servo = 1.0;
    front_servo = 1.0;
    back_thrust = 1.0;
    front_thrust = 1.0;
    wait(0.1);
    back_servo = 0.0;
    front_servo = 0.0;
    back_thrust = 0.0;
    front_thrust = 0.0;
    wait(0.1);
    //End calibration sequence
    front_thrust = 0.46;
    back_thrust = 0.46;
    back_servo = 0.5;
    front_servo = 0.5;
}


//sends command message to led controller for LED
void ledSend(float ledColorOut, float ledBrightOut){
    /* How to use:
    -First input is for the color, second for brightness
    
    -Brightness goes from 0 to 100 (float, so it includes decimal)
    
    -Color code:
        0: off
        1: red
        2: green
        3: blue
        4: yellow
        5: purple
        Anything else turns it off as a failsafe
        It's a float value but only give it whole numbers, I just didn't feel like having 2 unions
    */
    
    //initializing values
    int numsend=0;
    char buf[30];
    
    //create message
    buf[0] = 0xFF;  //header
    buf[1] = 0x00;  //message ID
    
    //take the color, and using the union, turn it into bytes
    float_union.f = ledColorOut;
    buf[2] = float_union.bytes[0];
    buf[3] = float_union.bytes[1];
    buf[4] = float_union.bytes[2];
    buf[5] = float_union.bytes[3];
    
    //do the same with brightness
    float_union.f = ledBrightOut;
    buf[6] = float_union.bytes[0];
    buf[7] = float_union.bytes[1];
    buf[8] = float_union.bytes[2];
    buf[9] = float_union.bytes[3];
    
    //send the message over serial
    while(numsend < 10){            
        ledControl.putc(buf[numsend]);
        numsend++;
    }
}

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////        Main        ///////////////////////////////
////////////////////////////////////////////////////////////////////////////////

int main() {
    
    Calibrate();
    
    pc.attach(&onSerialRx);
    unsigned int expired = 0;
    
    ledTime.start();
    nickTime.start();
    ledSend(0,0);
    
    int stopFlag = 0;
    
    Ctrl.mode(PullDown);
    
    
    
    while(1) {
        
        //Enable Servo to turn 180 degrees
        back_servo.calibrate(0.00085,90.0);
        front_servo.calibrate(0.00085,90.0);
        
        
        //Read in all PWM signals and set them to a value between 0 and 1
        elevation_output = (elev.pulsewidth()*1000)-1;
        throttle_output = (thro.pulsewidth()*1000)-1;
        rudder_output = (rudd.pulsewidth()*1000)-1;
        aileron_output = (aile.pulsewidth()*1000)-1;
        
        //RC vs Auto PWM
        aux_output = (aux.pulsewidth()*1000)-1; // >.5 RC... <.5 Auto    
        
        //ESTOP PWM
        ESTOP_output = (gear.pulsewidth()*1000)-1;  // >.5 run... <.5 STOP
        
        
        //if(nickTime.read() > .1){
        //    pc.printf("%2.2f\t\r\n",Ctrl.read());
        //    nickTime.reset();
        //}
        
        stopFlag = 0;
        if(Ctrl.read() != 1){
            //stopped
            stopFlag++;
        }
        
        if(throttle_output < 0.75){
            //stopped
            stopFlag++;
        }
        
        
        
        
        if(ESTOP_output == (-1)){
            //controller turned off
            front_thrust = 0.46;
            back_thrust = 0.46;
            
            //led control for estop (1: red)
            ledColor = 1;
            ledBright = 100;
        }else{
            //controller is turned on
            if(stopFlag == 0){
                //if the estop button is not pressed
                if(ESTOP_output > 0.5){
                    //And if remote estop is not active     
                    if(aux_output > 0.5) //RC
                    {
                        //Servo Controllers
                        //back_servo = aileron_output;
                        back_servo = rudder_output;
                        
                        //Thrust Controllers
                        //back_thrust = throttle_output - 0.04;
                        back_thrust = elevation_output - 0.04;
                        
                        //led control for manual (4: yellow)
                        ledColor = 4;
                        ledBright = 75;
                            
                    }
                    else //Auto
                    {   
                        if (newData)
                        {  
                            newData = false;//Reset NewData Boolean
                            
                            front_servo = inputs[0];//Set thruster values
                            front_thrust = RangeChange(inputs[1]) - 0.04;
                            back_servo = inputs[2];
                            back_thrust = RangeChange(inputs[3]) - 0.04;
                            
                            expired = 0;//Reset Expried
                        }
                        else
                        {
                            expired++; //Count the number of loops without new data
                        }
                
                        if(expired > 10000000) //If too many loops have passed with no new data, assume Labview crashed
                        {
                            back_thrust = 0.46;
                            front_thrust = 0.46;
                            
                            //led control for loss of labview (5: purple)
                            ledColor = 5;
                            ledBright = 100;
                        } 
                        
                        //led control for auto (2: green, 3: blue)
                        ledColor = 2;  
                        ledBright = 75;              
                    }
                }
                else
                {
                    front_thrust = 0.46;
                    back_thrust = 0.46;
                    
                    //led control for estop (1: red)
                    ledColor = 1;
                    ledBright = 100;
                    
                }  
            }else{
                //estop button override
                front_thrust = 0.46;
                back_thrust = 0.46;
                
                //led control for estop (1: red)
                ledColor = 1;
                ledBright = 100;
            }
        }
        if(ledTime > 0.5){
            //only sends every half second to prevent explosions
            ledSend(ledColor,ledBright);
            ledTime.reset();
        }       
    }
}