estop button and light tower
Dependencies: PwmIn mbed Servo
main.cpp
- Committer:
- WoodyERAU
- Date:
- 2019-06-19
- Revision:
- 0:d18628ed3ec3
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(); } } }