Roboboat 2019
estop button and light tower
Dependencies: PwmIn mbed Servo
Diff: main.cpp
- Revision:
- 0:d18628ed3ec3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jun 19 02:46:40 2019 +0000
@@ -0,0 +1,333 @@
+#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();
+ }
+ }
+}
+