![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |