main.cpp

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