Revision 0:c1b1717df71a, committed 2022-06-20

Comitter:

raerroboboat

Date:

Mon Jun 20 14:42:11 2022 +0000

Commit message:

for coyle

Changed in this revision

diff -r 000000000000 -r c1b1717df71a PwmIn.lib
--- /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

diff -r 000000000000 -r c1b1717df71a Servo.lib
--- /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

diff -r 000000000000 -r c1b1717df71a mbed.bld
--- /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

diff -r 000000000000 -r c1b1717df71a src/main.cpp
--- /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();
+        }       
+    }
+}