![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed stuffs
Dependencies: PwmIn mbed Servo
src/main.cpp
- Committer:
- raerroboboat
- Date:
- 2022-06-20
- Revision:
- 0:c1b1717df71a
File content as of revision 0:c1b1717df71a:
/*Roboboat Thruster Control Module for Mbed LPC1768 Written by Andrew Wood, Nick Middlebrooks, Sean Cannon */ #include "mbed.h" #include "Servo.h" #include "PwmIn.h" ///////////////////////////// setup ////////////////////////////// //Define and Initialize left and right servos Servo back_servo(p23); Servo front_servo(p24); //E-Stop Button DigitalIn Ctrl(p8); DigitalOut EstopControl(p10); //Define and Initalize front and Back thrusters Servo front_thrust(p25); Servo back_thrust(p26); //Initialize receiver pins /* PwmIn aux(p18); //RC to AUTO SWITCH, Pin 1 on Spectrum PwmIn thro(p17); //Pin 2 on Spectrum PwmIn rudd(p16); //Pin 3 on Spectrum PwmIn elev(p15); //Pin 4 on Spectrum PwmIn aile(p14); //Pin 5 on Spectrum PwmIn gear(p13); //ESTOP, Pin 6 on Spectrum */ PwmIn aux(p13); //RC to AUTO SWITCH, Pin 1 on Spectrum PwmIn thro(p18); //Pin 2 on Spectrum PwmIn rudd(p17); //Pin 3 on Spectrum PwmIn elev(p16); //Pin 4 on Spectrum PwmIn aile(p15); //Pin 5 on Spectrum PwmIn gear(p14); //ESTOP, Pin 6 on Spectrum //define serial communication (probably going to be disabled for UDP instead) Serial pc(USBTX, USBRX); Serial ledControl(p28, p27); Timer nickTime; Timer ledTime; ///////////////////////////// global variables ////////////////////////////// //Set all PWM values to a default 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; EstopControl = 0; } //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 */ //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(void) { 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++; } //ESTOP Logic if(ESTOP_output == (-1)) { //controller turned off front_thrust = 0.46; back_thrust = 0.46; //EstopControl = 0; //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 //EstopControl = 1; if(ESTOP_output > 0.5){ //And if remote estop is not active if(aux_output > 0.5) //RC { //Servo Controllers back_servo = rudder_output; //Thrust Controllers back_thrust = elevation_output - 0.04; //led control for manual (2: green) ledColor = 2; ledBright = 100; } 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) Notes: Now blue due to new comp rules ledColor = 3; ledBright = 100; } } 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(); } } }