mbed stuffs

Dependencies:   PwmIn mbed Servo

Files at this revision

API Documentation at this revision

Comitter:
raerroboboat
Date:
Mon Jun 20 14:42:11 2022 +0000
Commit message:
for coyle

Changed in this revision

PwmIn.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
src/main.cpp Show annotated file Show diff for this revision Revisions of this file
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();
+        }       
+    }
+}