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?
User | Revision | Line number | New 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 |