Dominic Marsh
mbed stuffs
Dependencies: PwmIn mbed Servo
Revision 0:c1b1717df71a, committed 23 months ago
- Comitter:
- raerroboboat
- Date:
- Mon Jun 20 14:42:11 2022 +0000
- Commit message:
- for coyle
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn.lib Mon Jun 20 14:42:11 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/PwmIn/#6d68eb9b6bbb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Jun 20 14:42:11 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jun 20 14:42:11 2022 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/main.cpp Mon Jun 20 14:42:11 2022 +0000
@@ -0,0 +1,352 @@
+/*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();
+ }
+ }
+}