Dominic Marsh / Mbed 2 deprecated roboboat2022

mbed stuffs

Dependencies:   PwmIn mbed Servo

src/main.cpp@0:c1b1717df71a, 2022-06-20 (annotated)

Committer:
raerroboboat
Date:
Mon Jun 20 14:42:11 2022 +0000
Revision:
0:c1b1717df71a
for coyle

Who changed what in which revision?

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