Roboboat 2019 / Mbed 2 deprecated RoboBoat2019Final

estop button and light tower

Dependencies:   PwmIn mbed Servo

main.cpp@0:d18628ed3ec3, 2019-06-19 (annotated)

Committer:
WoodyERAU
Date:
Wed Jun 19 02:46:40 2019 +0000
Revision:
0:d18628ed3ec3
fixed light tower and estop button

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WoodyERAU 0:d18628ed3ec3 1 #include "mbed.h"
WoodyERAU 0:d18628ed3ec3 2 #include "Servo.h"
WoodyERAU 0:d18628ed3ec3 3 #include "PwmIn.h"
WoodyERAU 0:d18628ed3ec3 4
WoodyERAU 0:d18628ed3ec3 5 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 6 ///////////////////////////// setup //////////////////////////////
WoodyERAU 0:d18628ed3ec3 7 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 8
WoodyERAU 0:d18628ed3ec3 9 //Initialize left and right servos
WoodyERAU 0:d18628ed3ec3 10 Servo back_servo(p23);
WoodyERAU 0:d18628ed3ec3 11 Servo front_servo(p24);
WoodyERAU 0:d18628ed3ec3 12
WoodyERAU 0:d18628ed3ec3 13 //E-Stop Button
WoodyERAU 0:d18628ed3ec3 14 DigitalIn Ctrl(p8);
WoodyERAU 0:d18628ed3ec3 15
WoodyERAU 0:d18628ed3ec3 16 //Pins for Thruster Power
WoodyERAU 0:d18628ed3ec3 17 Servo front_thrust(p25);
WoodyERAU 0:d18628ed3ec3 18 Servo back_thrust(p26);
WoodyERAU 0:d18628ed3ec3 19
WoodyERAU 0:d18628ed3ec3 20 //Initialize receiver pins
WoodyERAU 0:d18628ed3ec3 21 PwmIn thro(p17);
WoodyERAU 0:d18628ed3ec3 22 PwmIn elev(p15);
WoodyERAU 0:d18628ed3ec3 23 PwmIn gear(p13);
WoodyERAU 0:d18628ed3ec3 24 PwmIn aile(p14);
WoodyERAU 0:d18628ed3ec3 25 PwmIn rudd(p16);
WoodyERAU 0:d18628ed3ec3 26 PwmIn aux(p18);
WoodyERAU 0:d18628ed3ec3 27
WoodyERAU 0:d18628ed3ec3 28 Serial pc(USBTX, USBRX);
WoodyERAU 0:d18628ed3ec3 29 Serial ledControl(p28, p27);
WoodyERAU 0:d18628ed3ec3 30
WoodyERAU 0:d18628ed3ec3 31 Timer nickTime;
WoodyERAU 0:d18628ed3ec3 32 Timer ledTime;
WoodyERAU 0:d18628ed3ec3 33
WoodyERAU 0:d18628ed3ec3 34 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 35 ///////////////////////////// global variables //////////////////////////////
WoodyERAU 0:d18628ed3ec3 36 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 37
WoodyERAU 0:d18628ed3ec3 38 //Set all PWM values to a defaalt of 0
WoodyERAU 0:d18628ed3ec3 39 float throttle_output = 0.0;
WoodyERAU 0:d18628ed3ec3 40 float elevation_output = 0.0;
WoodyERAU 0:d18628ed3ec3 41 float ESTOP_output = 0.0;
WoodyERAU 0:d18628ed3ec3 42 float rudder_output = 0.0;
WoodyERAU 0:d18628ed3ec3 43 float aileron_output = 0.0;
WoodyERAU 0:d18628ed3ec3 44 float aux_output = 0.0;
WoodyERAU 0:d18628ed3ec3 45
WoodyERAU 0:d18628ed3ec3 46 //Variables for Serial Communcation with Labview
WoodyERAU 0:d18628ed3ec3 47 volatile bool newData = false;
WoodyERAU 0:d18628ed3ec3 48 volatile float inputs[4];
WoodyERAU 0:d18628ed3ec3 49
WoodyERAU 0:d18628ed3ec3 50 //light tower
WoodyERAU 0:d18628ed3ec3 51 float ledColor = 0;
WoodyERAU 0:d18628ed3ec3 52 float ledBright = 0;
WoodyERAU 0:d18628ed3ec3 53
WoodyERAU 0:d18628ed3ec3 54 //union for things in the light tower
WoodyERAU 0:d18628ed3ec3 55 union {
WoodyERAU 0:d18628ed3ec3 56 float f;
WoodyERAU 0:d18628ed3ec3 57 char bytes[4];
WoodyERAU 0:d18628ed3ec3 58 } float_union;
WoodyERAU 0:d18628ed3ec3 59
WoodyERAU 0:d18628ed3ec3 60 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 61 ///////////////////////////// Functions ///////////////////////////////
WoodyERAU 0:d18628ed3ec3 62 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 63
WoodyERAU 0:d18628ed3ec3 64 //Function changes 0 t 1 input into 0.08 to 0.92
WoodyERAU 0:d18628ed3ec3 65 float RangeChange(float x)
WoodyERAU 0:d18628ed3ec3 66 {
WoodyERAU 0:d18628ed3ec3 67 float y;
WoodyERAU 0:d18628ed3ec3 68 y = ((x * 0.84) + 0.08);
WoodyERAU 0:d18628ed3ec3 69 return y;
WoodyERAU 0:d18628ed3ec3 70 }
WoodyERAU 0:d18628ed3ec3 71
WoodyERAU 0:d18628ed3ec3 72
WoodyERAU 0:d18628ed3ec3 73 //Function Reads Serial com
WoodyERAU 0:d18628ed3ec3 74 void onSerialRx()
WoodyERAU 0:d18628ed3ec3 75 {
WoodyERAU 0:d18628ed3ec3 76 static char serialInBuffer[100];
WoodyERAU 0:d18628ed3ec3 77 static int serialCount = 0;
WoodyERAU 0:d18628ed3ec3 78
WoodyERAU 0:d18628ed3ec3 79 while (pc.readable())// in case two bytes are ready
WoodyERAU 0:d18628ed3ec3 80 {
WoodyERAU 0:d18628ed3ec3 81 char byteIn = pc.getc();//Read serial message
WoodyERAU 0:d18628ed3ec3 82
WoodyERAU 0:d18628ed3ec3 83 if (byteIn == 0x65)// if an end of line is found
WoodyERAU 0:d18628ed3ec3 84 {
WoodyERAU 0:d18628ed3ec3 85 serialInBuffer[serialCount] == 0; // null terminate the input
WoodyERAU 0:d18628ed3ec3 86 float w,x,y,z;
WoodyERAU 0:d18628ed3ec3 87 if (sscanf(serialInBuffer,"%f,%f,%f,%fe",&w,&x,&y,&z) == 4)// managed to read all 4 values
WoodyERAU 0:d18628ed3ec3 88 {
WoodyERAU 0:d18628ed3ec3 89 inputs[0] = w;
WoodyERAU 0:d18628ed3ec3 90 inputs[1] = x;
WoodyERAU 0:d18628ed3ec3 91 inputs[2] = y;
WoodyERAU 0:d18628ed3ec3 92 inputs[3] = z;
WoodyERAU 0:d18628ed3ec3 93 newData = true;
WoodyERAU 0:d18628ed3ec3 94 }
WoodyERAU 0:d18628ed3ec3 95 serialCount = 0; // reset the buffer
WoodyERAU 0:d18628ed3ec3 96 }
WoodyERAU 0:d18628ed3ec3 97 else
WoodyERAU 0:d18628ed3ec3 98 {
WoodyERAU 0:d18628ed3ec3 99 serialInBuffer[serialCount] = byteIn;// store the character
WoodyERAU 0:d18628ed3ec3 100 if (serialCount<100)
WoodyERAU 0:d18628ed3ec3 101 {
WoodyERAU 0:d18628ed3ec3 102 serialCount++;// increase the counter.
WoodyERAU 0:d18628ed3ec3 103 }
WoodyERAU 0:d18628ed3ec3 104 }
WoodyERAU 0:d18628ed3ec3 105 }
WoodyERAU 0:d18628ed3ec3 106 }
WoodyERAU 0:d18628ed3ec3 107
WoodyERAU 0:d18628ed3ec3 108
WoodyERAU 0:d18628ed3ec3 109 void Calibrate()
WoodyERAU 0:d18628ed3ec3 110 {
WoodyERAU 0:d18628ed3ec3 111 //Calibration Sequence
WoodyERAU 0:d18628ed3ec3 112 back_servo = 0.0;
WoodyERAU 0:d18628ed3ec3 113 front_servo = 0.0;
WoodyERAU 0:d18628ed3ec3 114 back_thrust = 0.0;
WoodyERAU 0:d18628ed3ec3 115 front_thrust = 0.0;
WoodyERAU 0:d18628ed3ec3 116 wait(0.1); //ESC detects signal
WoodyERAU 0:d18628ed3ec3 117 //Required ESC Calibration/Arming sequence
WoodyERAU 0:d18628ed3ec3 118 //sends longest and shortest PWM pulse to learn and arm at power on
WoodyERAU 0:d18628ed3ec3 119 back_servo = 1.0;
WoodyERAU 0:d18628ed3ec3 120 front_servo = 1.0;
WoodyERAU 0:d18628ed3ec3 121 back_thrust = 1.0;
WoodyERAU 0:d18628ed3ec3 122 front_thrust = 1.0;
WoodyERAU 0:d18628ed3ec3 123 wait(0.1);
WoodyERAU 0:d18628ed3ec3 124 back_servo = 0.0;
WoodyERAU 0:d18628ed3ec3 125 front_servo = 0.0;
WoodyERAU 0:d18628ed3ec3 126 back_thrust = 0.0;
WoodyERAU 0:d18628ed3ec3 127 front_thrust = 0.0;
WoodyERAU 0:d18628ed3ec3 128 wait(0.1);
WoodyERAU 0:d18628ed3ec3 129 //End calibration sequence
WoodyERAU 0:d18628ed3ec3 130 front_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 131 back_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 132 back_servo = 0.5;
WoodyERAU 0:d18628ed3ec3 133 front_servo = 0.5;
WoodyERAU 0:d18628ed3ec3 134 }
WoodyERAU 0:d18628ed3ec3 135
WoodyERAU 0:d18628ed3ec3 136
WoodyERAU 0:d18628ed3ec3 137 //sends command message to led controller for LED
WoodyERAU 0:d18628ed3ec3 138 void ledSend(float ledColorOut, float ledBrightOut){
WoodyERAU 0:d18628ed3ec3 139 /* How to use:
WoodyERAU 0:d18628ed3ec3 140 -First input is for the color, second for brightness
WoodyERAU 0:d18628ed3ec3 141
WoodyERAU 0:d18628ed3ec3 142 -Brightness goes from 0 to 100 (float, so it includes decimal)
WoodyERAU 0:d18628ed3ec3 143
WoodyERAU 0:d18628ed3ec3 144 -Color code:
WoodyERAU 0:d18628ed3ec3 145 0: off
WoodyERAU 0:d18628ed3ec3 146 1: red
WoodyERAU 0:d18628ed3ec3 147 2: green
WoodyERAU 0:d18628ed3ec3 148 3: blue
WoodyERAU 0:d18628ed3ec3 149 4: yellow
WoodyERAU 0:d18628ed3ec3 150 5: purple
WoodyERAU 0:d18628ed3ec3 151 Anything else turns it off as a failsafe
WoodyERAU 0:d18628ed3ec3 152 It's a float value but only give it whole numbers, I just didn't feel like having 2 unions
WoodyERAU 0:d18628ed3ec3 153 */
WoodyERAU 0:d18628ed3ec3 154
WoodyERAU 0:d18628ed3ec3 155 //initializing values
WoodyERAU 0:d18628ed3ec3 156 int numsend=0;
WoodyERAU 0:d18628ed3ec3 157 char buf[30];
WoodyERAU 0:d18628ed3ec3 158
WoodyERAU 0:d18628ed3ec3 159 //create message
WoodyERAU 0:d18628ed3ec3 160 buf[0] = 0xFF; //header
WoodyERAU 0:d18628ed3ec3 161 buf[1] = 0x00; //message ID
WoodyERAU 0:d18628ed3ec3 162
WoodyERAU 0:d18628ed3ec3 163 //take the color, and using the union, turn it into bytes
WoodyERAU 0:d18628ed3ec3 164 float_union.f = ledColorOut;
WoodyERAU 0:d18628ed3ec3 165 buf[2] = float_union.bytes[0];
WoodyERAU 0:d18628ed3ec3 166 buf[3] = float_union.bytes[1];
WoodyERAU 0:d18628ed3ec3 167 buf[4] = float_union.bytes[2];
WoodyERAU 0:d18628ed3ec3 168 buf[5] = float_union.bytes[3];
WoodyERAU 0:d18628ed3ec3 169
WoodyERAU 0:d18628ed3ec3 170 //do the same with brightness
WoodyERAU 0:d18628ed3ec3 171 float_union.f = ledBrightOut;
WoodyERAU 0:d18628ed3ec3 172 buf[6] = float_union.bytes[0];
WoodyERAU 0:d18628ed3ec3 173 buf[7] = float_union.bytes[1];
WoodyERAU 0:d18628ed3ec3 174 buf[8] = float_union.bytes[2];
WoodyERAU 0:d18628ed3ec3 175 buf[9] = float_union.bytes[3];
WoodyERAU 0:d18628ed3ec3 176
WoodyERAU 0:d18628ed3ec3 177 //send the message over serial
WoodyERAU 0:d18628ed3ec3 178 while(numsend < 10){
WoodyERAU 0:d18628ed3ec3 179 ledControl.putc(buf[numsend]);
WoodyERAU 0:d18628ed3ec3 180 numsend++;
WoodyERAU 0:d18628ed3ec3 181 }
WoodyERAU 0:d18628ed3ec3 182 }
WoodyERAU 0:d18628ed3ec3 183
WoodyERAU 0:d18628ed3ec3 184 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 185 ///////////////////////////// Main ///////////////////////////////
WoodyERAU 0:d18628ed3ec3 186 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 0:d18628ed3ec3 187
WoodyERAU 0:d18628ed3ec3 188 int main() {
WoodyERAU 0:d18628ed3ec3 189
WoodyERAU 0:d18628ed3ec3 190 Calibrate();
WoodyERAU 0:d18628ed3ec3 191
WoodyERAU 0:d18628ed3ec3 192 pc.attach(&onSerialRx);
WoodyERAU 0:d18628ed3ec3 193 unsigned int expired = 0;
WoodyERAU 0:d18628ed3ec3 194
WoodyERAU 0:d18628ed3ec3 195 ledTime.start();
WoodyERAU 0:d18628ed3ec3 196 nickTime.start();
WoodyERAU 0:d18628ed3ec3 197 ledSend(0,0);
WoodyERAU 0:d18628ed3ec3 198
WoodyERAU 0:d18628ed3ec3 199 int stopFlag = 0;
WoodyERAU 0:d18628ed3ec3 200
WoodyERAU 0:d18628ed3ec3 201 Ctrl.mode(PullDown);
WoodyERAU 0:d18628ed3ec3 202
WoodyERAU 0:d18628ed3ec3 203
WoodyERAU 0:d18628ed3ec3 204
WoodyERAU 0:d18628ed3ec3 205 while(1) {
WoodyERAU 0:d18628ed3ec3 206
WoodyERAU 0:d18628ed3ec3 207 //Enable Servo to turn 180 degrees
WoodyERAU 0:d18628ed3ec3 208 back_servo.calibrate(0.00085,90.0);
WoodyERAU 0:d18628ed3ec3 209 front_servo.calibrate(0.00085,90.0);
WoodyERAU 0:d18628ed3ec3 210
WoodyERAU 0:d18628ed3ec3 211
WoodyERAU 0:d18628ed3ec3 212 //Read in all PWM signals and set them to a value between 0 and 1
WoodyERAU 0:d18628ed3ec3 213 elevation_output = (elev.pulsewidth()*1000)-1;
WoodyERAU 0:d18628ed3ec3 214 throttle_output = (thro.pulsewidth()*1000)-1;
WoodyERAU 0:d18628ed3ec3 215 rudder_output = (rudd.pulsewidth()*1000)-1;
WoodyERAU 0:d18628ed3ec3 216 aileron_output = (aile.pulsewidth()*1000)-1;
WoodyERAU 0:d18628ed3ec3 217
WoodyERAU 0:d18628ed3ec3 218 //RC vs Auto PWM
WoodyERAU 0:d18628ed3ec3 219 aux_output = (aux.pulsewidth()*1000)-1; // >.5 RC... <.5 Auto
WoodyERAU 0:d18628ed3ec3 220
WoodyERAU 0:d18628ed3ec3 221 //ESTOP PWM
WoodyERAU 0:d18628ed3ec3 222 ESTOP_output = (gear.pulsewidth()*1000)-1; // >.5 run... <.5 STOP
WoodyERAU 0:d18628ed3ec3 223
WoodyERAU 0:d18628ed3ec3 224
WoodyERAU 0:d18628ed3ec3 225 //if(nickTime.read() > .1){
WoodyERAU 0:d18628ed3ec3 226 // pc.printf("%2.2f\t\r\n",Ctrl.read());
WoodyERAU 0:d18628ed3ec3 227 // nickTime.reset();
WoodyERAU 0:d18628ed3ec3 228 //}
WoodyERAU 0:d18628ed3ec3 229
WoodyERAU 0:d18628ed3ec3 230 stopFlag = 0;
WoodyERAU 0:d18628ed3ec3 231 if(Ctrl.read() != 1){
WoodyERAU 0:d18628ed3ec3 232 //stopped
WoodyERAU 0:d18628ed3ec3 233 stopFlag++;
WoodyERAU 0:d18628ed3ec3 234 }
WoodyERAU 0:d18628ed3ec3 235
WoodyERAU 0:d18628ed3ec3 236 if(throttle_output < 0.75){
WoodyERAU 0:d18628ed3ec3 237 //stopped
WoodyERAU 0:d18628ed3ec3 238 stopFlag++;
WoodyERAU 0:d18628ed3ec3 239 }
WoodyERAU 0:d18628ed3ec3 240
WoodyERAU 0:d18628ed3ec3 241
WoodyERAU 0:d18628ed3ec3 242
WoodyERAU 0:d18628ed3ec3 243
WoodyERAU 0:d18628ed3ec3 244 if(ESTOP_output == (-1)){
WoodyERAU 0:d18628ed3ec3 245 //controller turned off
WoodyERAU 0:d18628ed3ec3 246 front_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 247 back_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 248
WoodyERAU 0:d18628ed3ec3 249 //led control for estop (1: red)
WoodyERAU 0:d18628ed3ec3 250 ledColor = 1;
WoodyERAU 0:d18628ed3ec3 251 ledBright = 100;
WoodyERAU 0:d18628ed3ec3 252 }else{
WoodyERAU 0:d18628ed3ec3 253 //controller is turned on
WoodyERAU 0:d18628ed3ec3 254 if(stopFlag == 0){
WoodyERAU 0:d18628ed3ec3 255 //if the estop button is not pressed
WoodyERAU 0:d18628ed3ec3 256 if(ESTOP_output > 0.5){
WoodyERAU 0:d18628ed3ec3 257 //And if remote estop is not active
WoodyERAU 0:d18628ed3ec3 258 if(aux_output > 0.5) //RC
WoodyERAU 0:d18628ed3ec3 259 {
WoodyERAU 0:d18628ed3ec3 260 //Servo Controllers
WoodyERAU 0:d18628ed3ec3 261 //back_servo = aileron_output;
WoodyERAU 0:d18628ed3ec3 262 back_servo = rudder_output;
WoodyERAU 0:d18628ed3ec3 263
WoodyERAU 0:d18628ed3ec3 264 //Thrust Controllers
WoodyERAU 0:d18628ed3ec3 265 //back_thrust = throttle_output - 0.04;
WoodyERAU 0:d18628ed3ec3 266 back_thrust = elevation_output - 0.04;
WoodyERAU 0:d18628ed3ec3 267
WoodyERAU 0:d18628ed3ec3 268 //led control for manual (4: yellow)
WoodyERAU 0:d18628ed3ec3 269 ledColor = 4;
WoodyERAU 0:d18628ed3ec3 270 ledBright = 75;
WoodyERAU 0:d18628ed3ec3 271
WoodyERAU 0:d18628ed3ec3 272 }
WoodyERAU 0:d18628ed3ec3 273 else //Auto
WoodyERAU 0:d18628ed3ec3 274 {
WoodyERAU 0:d18628ed3ec3 275 if (newData)
WoodyERAU 0:d18628ed3ec3 276 {
WoodyERAU 0:d18628ed3ec3 277 newData = false;//Reset NewData Boolean
WoodyERAU 0:d18628ed3ec3 278
WoodyERAU 0:d18628ed3ec3 279 front_servo = inputs[0];//Set thruster values
WoodyERAU 0:d18628ed3ec3 280 front_thrust = RangeChange(inputs[1]) - 0.04;
WoodyERAU 0:d18628ed3ec3 281 back_servo = inputs[2];
WoodyERAU 0:d18628ed3ec3 282 back_thrust = RangeChange(inputs[3]) - 0.04;
WoodyERAU 0:d18628ed3ec3 283
WoodyERAU 0:d18628ed3ec3 284 expired = 0;//Reset Expried
WoodyERAU 0:d18628ed3ec3 285 }
WoodyERAU 0:d18628ed3ec3 286 else
WoodyERAU 0:d18628ed3ec3 287 {
WoodyERAU 0:d18628ed3ec3 288 expired++; //Count the number of loops without new data
WoodyERAU 0:d18628ed3ec3 289 }
WoodyERAU 0:d18628ed3ec3 290
WoodyERAU 0:d18628ed3ec3 291 if(expired > 10000000) //If too many loops have passed with no new data, assume Labview crashed
WoodyERAU 0:d18628ed3ec3 292 {
WoodyERAU 0:d18628ed3ec3 293 back_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 294 front_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 295
WoodyERAU 0:d18628ed3ec3 296 //led control for loss of labview (5: purple)
WoodyERAU 0:d18628ed3ec3 297 ledColor = 5;
WoodyERAU 0:d18628ed3ec3 298 ledBright = 100;
WoodyERAU 0:d18628ed3ec3 299 }
WoodyERAU 0:d18628ed3ec3 300
WoodyERAU 0:d18628ed3ec3 301 //led control for auto (2: green, 3: blue)
WoodyERAU 0:d18628ed3ec3 302 ledColor = 2;
WoodyERAU 0:d18628ed3ec3 303 ledBright = 75;
WoodyERAU 0:d18628ed3ec3 304 }
WoodyERAU 0:d18628ed3ec3 305 }
WoodyERAU 0:d18628ed3ec3 306 else
WoodyERAU 0:d18628ed3ec3 307 {
WoodyERAU 0:d18628ed3ec3 308 front_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 309 back_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 310
WoodyERAU 0:d18628ed3ec3 311 //led control for estop (1: red)
WoodyERAU 0:d18628ed3ec3 312 ledColor = 1;
WoodyERAU 0:d18628ed3ec3 313 ledBright = 100;
WoodyERAU 0:d18628ed3ec3 314
WoodyERAU 0:d18628ed3ec3 315 }
WoodyERAU 0:d18628ed3ec3 316 }else{
WoodyERAU 0:d18628ed3ec3 317 //estop button override
WoodyERAU 0:d18628ed3ec3 318 front_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 319 back_thrust = 0.46;
WoodyERAU 0:d18628ed3ec3 320
WoodyERAU 0:d18628ed3ec3 321 //led control for estop (1: red)
WoodyERAU 0:d18628ed3ec3 322 ledColor = 1;
WoodyERAU 0:d18628ed3ec3 323 ledBright = 100;
WoodyERAU 0:d18628ed3ec3 324 }
WoodyERAU 0:d18628ed3ec3 325 }
WoodyERAU 0:d18628ed3ec3 326 if(ledTime > 0.5){
WoodyERAU 0:d18628ed3ec3 327 //only sends every half second to prevent explosions
WoodyERAU 0:d18628ed3ec3 328 ledSend(ledColor,ledBright);
WoodyERAU 0:d18628ed3ec3 329 ledTime.reset();
WoodyERAU 0:d18628ed3ec3 330 }
WoodyERAU 0:d18628ed3ec3 331 }
WoodyERAU 0:d18628ed3ec3 332 }
WoodyERAU 0:d18628ed3ec3 333