main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "PwmIn.h"
00004 
00005 ////////////////////////////////////////////////////////////////////////////////
00006 /////////////////////////////        setup        //////////////////////////////
00007 ////////////////////////////////////////////////////////////////////////////////
00008 
00009 //Initialize left and right servos
00010 Servo back_servo(p23);
00011 Servo front_servo(p24);
00012 
00013 //E-Stop Button
00014 DigitalIn Ctrl(p8);
00015 
00016 //Pins for Thruster Power
00017 Servo front_thrust(p25);
00018 Servo back_thrust(p26);
00019 
00020 //Initialize receiver pins
00021 PwmIn thro(p17);
00022 PwmIn elev(p15);
00023 PwmIn gear(p13);
00024 PwmIn aile(p14);
00025 PwmIn rudd(p16);
00026 PwmIn aux(p18);
00027 
00028 Serial pc(USBTX, USBRX);
00029 Serial ledControl(p28, p27);
00030 
00031 Timer nickTime;
00032 Timer ledTime;
00033 
00034 ////////////////////////////////////////////////////////////////////////////////
00035 /////////////////////////////  global variables   //////////////////////////////
00036 ////////////////////////////////////////////////////////////////////////////////
00037 
00038 //Set all PWM values to a defaalt of 0
00039 float throttle_output = 0.0;
00040 float elevation_output = 0.0;
00041 float ESTOP_output = 0.0;
00042 float rudder_output = 0.0;
00043 float aileron_output = 0.0;
00044 float aux_output = 0.0;
00045 
00046 //Variables for Serial Communcation with Labview
00047 volatile bool newData = false;
00048 volatile float inputs[4];
00049 
00050 //light tower
00051 float ledColor = 0;
00052 float ledBright = 0;
00053 
00054 //union for things in the light tower
00055 union {
00056   float f;
00057   char  bytes[4];
00058 } float_union;
00059 
00060 ////////////////////////////////////////////////////////////////////////////////
00061 /////////////////////////////     Functions      ///////////////////////////////
00062 ////////////////////////////////////////////////////////////////////////////////
00063 
00064 //Function changes 0 t 1 input into 0.08 to 0.92
00065 float RangeChange(float x)
00066 {
00067     float y;
00068     y = ((x * 0.84) + 0.08);
00069     return y;
00070 }
00071 
00072 
00073 //Function Reads Serial com
00074 void onSerialRx()
00075 {
00076     static char serialInBuffer[100]; 
00077     static int serialCount = 0;
00078  
00079     while (pc.readable())// in case two bytes are ready
00080     {
00081         char byteIn = pc.getc();//Read serial message
00082         
00083         if (byteIn == 0x65)// if an end of line is found
00084         { 
00085             serialInBuffer[serialCount] == 0; // null terminate the input                
00086             float w,x,y,z;
00087             if (sscanf(serialInBuffer,"%f,%f,%f,%fe",&w,&x,&y,&z) == 4)// managed to read all 4 values
00088             { 
00089                 inputs[0] = w;
00090                 inputs[1] = x;
00091                 inputs[2] = y;
00092                 inputs[3] = z;
00093                 newData = true;
00094             }
00095             serialCount = 0; // reset the buffer
00096         } 
00097         else
00098         {
00099             serialInBuffer[serialCount] = byteIn;// store the character
00100             if (serialCount<100)
00101             {
00102                 serialCount++;// increase the counter.
00103             }
00104         }
00105     }
00106 }
00107 
00108 
00109 void Calibrate()
00110 {
00111     //Calibration Sequence
00112     back_servo = 0.0;
00113     front_servo = 0.0;
00114     back_thrust = 0.0;
00115     front_thrust = 0.0;
00116     wait(0.1); //ESC detects signal
00117     //Required ESC Calibration/Arming sequence  
00118     //sends longest and shortest PWM pulse to learn and arm at power on
00119     back_servo = 1.0;
00120     front_servo = 1.0;
00121     back_thrust = 1.0;
00122     front_thrust = 1.0;
00123     wait(0.1);
00124     back_servo = 0.0;
00125     front_servo = 0.0;
00126     back_thrust = 0.0;
00127     front_thrust = 0.0;
00128     wait(0.1);
00129     //End calibration sequence
00130     front_thrust = 0.46;
00131     back_thrust = 0.46;
00132     back_servo = 0.5;
00133     front_servo = 0.5;
00134 }
00135 
00136 
00137 //sends command message to led controller for LED
00138 void ledSend(float ledColorOut, float ledBrightOut){
00139     /* How to use:
00140     -First input is for the color, second for brightness
00141     
00142     -Brightness goes from 0 to 100 (float, so it includes decimal)
00143     
00144     -Color code:
00145         0: off
00146         1: red
00147         2: green
00148         3: blue
00149         4: yellow
00150         5: purple
00151         Anything else turns it off as a failsafe
00152         It's a float value but only give it whole numbers, I just didn't feel like having 2 unions
00153     */
00154     
00155     //initializing values
00156     int numsend=0;
00157     char buf[30];
00158     
00159     //create message
00160     buf[0] = 0xFF;  //header
00161     buf[1] = 0x00;  //message ID
00162     
00163     //take the color, and using the union, turn it into bytes
00164     float_union.f = ledColorOut;
00165     buf[2] = float_union.bytes[0];
00166     buf[3] = float_union.bytes[1];
00167     buf[4] = float_union.bytes[2];
00168     buf[5] = float_union.bytes[3];
00169     
00170     //do the same with brightness
00171     float_union.f = ledBrightOut;
00172     buf[6] = float_union.bytes[0];
00173     buf[7] = float_union.bytes[1];
00174     buf[8] = float_union.bytes[2];
00175     buf[9] = float_union.bytes[3];
00176     
00177     //send the message over serial
00178     while(numsend < 10){            
00179         ledControl.putc(buf[numsend]);
00180         numsend++;
00181     }
00182 }
00183 
00184 ////////////////////////////////////////////////////////////////////////////////
00185 /////////////////////////////        Main        ///////////////////////////////
00186 ////////////////////////////////////////////////////////////////////////////////
00187 
00188 int main() {
00189     
00190     Calibrate();
00191     
00192     pc.attach(&onSerialRx);
00193     unsigned int expired = 0;
00194     
00195     ledTime.start();
00196     nickTime.start();
00197     ledSend(0,0);
00198     
00199     int stopFlag = 0;
00200     
00201     Ctrl.mode(PullDown);
00202     
00203     
00204     
00205     while(1) {
00206         
00207         //Enable Servo to turn 180 degrees
00208         back_servo.calibrate(0.00085,90.0);
00209         front_servo.calibrate(0.00085,90.0);
00210         
00211         
00212         //Read in all PWM signals and set them to a value between 0 and 1
00213         elevation_output = (elev.pulsewidth()*1000)-1;
00214         throttle_output = (thro.pulsewidth()*1000)-1;
00215         rudder_output = (rudd.pulsewidth()*1000)-1;
00216         aileron_output = (aile.pulsewidth()*1000)-1;
00217         
00218         //RC vs Auto PWM
00219         aux_output = (aux.pulsewidth()*1000)-1; // >.5 RC... <.5 Auto    
00220         
00221         //ESTOP PWM
00222         ESTOP_output = (gear.pulsewidth()*1000)-1;  // >.5 run... <.5 STOP
00223         
00224         
00225         //if(nickTime.read() > .1){
00226         //    pc.printf("%2.2f\t\r\n",Ctrl.read());
00227         //    nickTime.reset();
00228         //}
00229         
00230         stopFlag = 0;
00231         if(Ctrl.read() != 1){
00232             //stopped
00233             stopFlag++;
00234         }
00235         
00236         if(throttle_output < 0.75){
00237             //stopped
00238             stopFlag++;
00239         }
00240         
00241         
00242         
00243         
00244         if(ESTOP_output == (-1)){
00245             //controller turned off
00246             front_thrust = 0.46;
00247             back_thrust = 0.46;
00248             
00249             //led control for estop (1: red)
00250             ledColor = 1;
00251             ledBright = 100;
00252         }else{
00253             //controller is turned on
00254             if(stopFlag == 0){
00255                 //if the estop button is not pressed
00256                 if(ESTOP_output > 0.5){
00257                     //And if remote estop is not active     
00258                     if(aux_output > 0.5) //RC
00259                     {
00260                         //Servo Controllers
00261                         //back_servo = aileron_output;
00262                         back_servo = rudder_output;
00263                         
00264                         //Thrust Controllers
00265                         //back_thrust = throttle_output - 0.04;
00266                         back_thrust = elevation_output - 0.04;
00267                         
00268                         //led control for manual (4: yellow)
00269                         ledColor = 4;
00270                         ledBright = 75;
00271                             
00272                     }
00273                     else //Auto
00274                     {   
00275                         if (newData)
00276                         {  
00277                             newData = false;//Reset NewData Boolean
00278                             
00279                             front_servo = inputs[0];//Set thruster values
00280                             front_thrust = RangeChange(inputs[1]) - 0.04;
00281                             back_servo = inputs[2];
00282                             back_thrust = RangeChange(inputs[3]) - 0.04;
00283                             
00284                             expired = 0;//Reset Expried
00285                         }
00286                         else
00287                         {
00288                             expired++; //Count the number of loops without new data
00289                         }
00290                 
00291                         if(expired > 10000000) //If too many loops have passed with no new data, assume Labview crashed
00292                         {
00293                             back_thrust = 0.46;
00294                             front_thrust = 0.46;
00295                             
00296                             //led control for loss of labview (5: purple)
00297                             ledColor = 5;
00298                             ledBright = 100;
00299                         } 
00300                         
00301                         //led control for auto (2: green, 3: blue)
00302                         ledColor = 2;  
00303                         ledBright = 75;              
00304                     }
00305                 }
00306                 else
00307                 {
00308                     front_thrust = 0.46;
00309                     back_thrust = 0.46;
00310                     
00311                     //led control for estop (1: red)
00312                     ledColor = 1;
00313                     ledBright = 100;
00314                     
00315                 }  
00316             }else{
00317                 //estop button override
00318                 front_thrust = 0.46;
00319                 back_thrust = 0.46;
00320                 
00321                 //led control for estop (1: red)
00322                 ledColor = 1;
00323                 ledBright = 100;
00324             }
00325         }
00326         if(ledTime > 0.5){
00327             //only sends every half second to prevent explosions
00328             ledSend(ledColor,ledBright);
00329             ledTime.reset();
00330         }       
00331     }
00332 }
00333