Mark Harris
/
SIEMENS_DLFEEDER_LOOP
Loop based controller for dual lane siemens siplace feeder.
feederController.cpp@1:4d3738338cf1, 2017-02-04 (annotated)
- Committer:
- Issus
- Date:
- Sat Feb 04 01:27:29 2017 +0000
- Revision:
- 1:4d3738338cf1
- Parent:
- 0:617334d8e3bb
Minimum on time for cover tape pickup. Added IAP for getting mcu serial number and future eeprom use. Moved defs to .h file.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Issus | 0:617334d8e3bb | 1 | #include "mbed.h" |
Issus | 0:617334d8e3bb | 2 | #include "USBSerial.h" |
Issus | 0:617334d8e3bb | 3 | #include "HBridge.h" |
Issus | 1:4d3738338cf1 | 4 | #include "feeder.h" |
Issus | 1:4d3738338cf1 | 5 | #include "IAP_LPC11U.h" |
Issus | 0:617334d8e3bb | 6 | |
Issus | 0:617334d8e3bb | 7 | extern "C" void mbed_mac_address(char *); |
Issus | 0:617334d8e3bb | 8 | |
Issus | 0:617334d8e3bb | 9 | /************ |
Issus | 0:617334d8e3bb | 10 | TODO LIST: |
Issus | 0:617334d8e3bb | 11 | ************* |
Issus | 0:617334d8e3bb | 12 | * printf needs to be replace with a function that putc instead for non-formatted strings |
Issus | 0:617334d8e3bb | 13 | * Convert lanes to a class so they there isn't duplicate code |
Issus | 1:4d3738338cf1 | 14 | * Store lane distance in EEPROM |
Issus | 1:4d3738338cf1 | 15 | * Store retrievable aribitrary text for the lane in EEPROM (loaded part, openpnp part feeder name, whatever) |
Issus | 0:617334d8e3bb | 16 | *************/ |
Issus | 0:617334d8e3bb | 17 | |
Issus | 0:617334d8e3bb | 18 | |
Issus | 0:617334d8e3bb | 19 | // SETTINGS |
Issus | 1:4d3738338cf1 | 20 | #define TAPE_MAX_PULL_TIME 3 // seconds |
Issus | 1:4d3738338cf1 | 21 | #define TAPE_MIN_PULL_TIME 1 // seconds |
Issus | 0:617334d8e3bb | 22 | |
Issus | 1:4d3738338cf1 | 23 | #define FEED_FWD_ENC_PULSE 2 // it's actually this value + 1 |
Issus | 0:617334d8e3bb | 24 | // encoder pulses: >(lane0FeedDistance * FEED_FWD_ENC_PULSE) |
Issus | 0:617334d8e3bb | 25 | // 3 for 1 position, 5 for 2 positions |
Issus | 0:617334d8e3bb | 26 | |
Issus | 0:617334d8e3bb | 27 | |
Issus | 0:617334d8e3bb | 28 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 29 | **** GLOBALS |
Issus | 0:617334d8e3bb | 30 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 31 | |
Issus | 0:617334d8e3bb | 32 | // SERIAL COMMAND INPUT |
Issus | 0:617334d8e3bb | 33 | enum command_t { READYFORCOMMAND, FEED, PICK, CLOSE, CONFIG }; |
Issus | 0:617334d8e3bb | 34 | command_t pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 35 | int pcCommandPos = 0; |
Issus | 0:617334d8e3bb | 36 | |
Issus | 0:617334d8e3bb | 37 | enum config_t { NONE, LANE }; |
Issus | 0:617334d8e3bb | 38 | config_t pcConfig = NONE; |
Issus | 0:617334d8e3bb | 39 | |
Issus | 0:617334d8e3bb | 40 | enum lane_t { UNSELECTED, ZERO, ONE }; |
Issus | 0:617334d8e3bb | 41 | lane_t selectedLane = UNSELECTED; |
Issus | 0:617334d8e3bb | 42 | |
Issus | 0:617334d8e3bb | 43 | // LANE STATES |
Issus | 0:617334d8e3bb | 44 | enum laneState_t { IDLE, PICKING, FEEDING, REVERSING }; |
Issus | 0:617334d8e3bb | 45 | |
Issus | 0:617334d8e3bb | 46 | laneState_t lane0State = IDLE; |
Issus | 0:617334d8e3bb | 47 | laneState_t lane1State = IDLE; |
Issus | 0:617334d8e3bb | 48 | |
Issus | 0:617334d8e3bb | 49 | // LANE EDGE STATES |
Issus | 0:617334d8e3bb | 50 | int lane0LastEdge = 0; |
Issus | 0:617334d8e3bb | 51 | int lane1LastEdge = 0; |
Issus | 0:617334d8e3bb | 52 | |
Issus | 0:617334d8e3bb | 53 | // LANE EDGE COUNTERS |
Issus | 0:617334d8e3bb | 54 | int lane0FeedEdge = 0; |
Issus | 0:617334d8e3bb | 55 | int lane1FeedEdge = 0; |
Issus | 0:617334d8e3bb | 56 | |
Issus | 0:617334d8e3bb | 57 | // NUMBER OF POSITIONS TO FEED THROUGH |
Issus | 0:617334d8e3bb | 58 | int lane0FeedDistance = 1; // 1 = 2mm, 2 = 4mm |
Issus | 0:617334d8e3bb | 59 | int lane1FeedDistance = 1; |
Issus | 0:617334d8e3bb | 60 | |
Issus | 0:617334d8e3bb | 61 | // FEEDER END BUTTON STATES |
Issus | 0:617334d8e3bb | 62 | int button0LastState = 0; |
Issus | 0:617334d8e3bb | 63 | int button1LastState = 0; |
Issus | 0:617334d8e3bb | 64 | |
Issus | 0:617334d8e3bb | 65 | // FEED STATE |
Issus | 0:617334d8e3bb | 66 | bool lane0Feed = 0; |
Issus | 0:617334d8e3bb | 67 | bool lane1Feed = 0; |
Issus | 0:617334d8e3bb | 68 | |
Issus | 0:617334d8e3bb | 69 | // PICKUP STATE |
Issus | 0:617334d8e3bb | 70 | bool tape0Takeup = 0; |
Issus | 0:617334d8e3bb | 71 | bool tape1Takeup = 0; |
Issus | 0:617334d8e3bb | 72 | |
Issus | 0:617334d8e3bb | 73 | // FEED STOPPER |
Issus | 0:617334d8e3bb | 74 | Timeout lane0Feedout; |
Issus | 0:617334d8e3bb | 75 | Timeout lane1Feedout; |
Issus | 0:617334d8e3bb | 76 | |
Issus | 1:4d3738338cf1 | 77 | // TAPE PICKUP STOPPER |
Issus | 1:4d3738338cf1 | 78 | Timer tape0PullTime; |
Issus | 1:4d3738338cf1 | 79 | Timer tape1PullTime; |
Issus | 0:617334d8e3bb | 80 | |
Issus | 0:617334d8e3bb | 81 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 82 | **** HARDWARE |
Issus | 0:617334d8e3bb | 83 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 84 | |
Issus | 0:617334d8e3bb | 85 | // SOLENOIDS (TAPE COVER) - keep above usb to turn off quickly |
Issus | 0:617334d8e3bb | 86 | DigitalOut SOLENOID_0(PIN_SOLCTL_0, 0); |
Issus | 0:617334d8e3bb | 87 | DigitalOut SOLENOID_1(PIN_SOLCTL_1, 0); |
Issus | 0:617334d8e3bb | 88 | |
Issus | 0:617334d8e3bb | 89 | // USB VCOM UART for MBED Serial Port Driver |
Issus | 0:617334d8e3bb | 90 | USBSerial pc(0x1f00, 0x2012, 0x0001, false); |
Issus | 0:617334d8e3bb | 91 | |
Issus | 0:617334d8e3bb | 92 | // RS485 |
Issus | 0:617334d8e3bb | 93 | //Serial control(PIN_UART_TXD, PIN_UART_RXD, 115200); |
Issus | 0:617334d8e3bb | 94 | DigitalOut RS485_DIR(PIN_RS485_DIR); |
Issus | 0:617334d8e3bb | 95 | |
Issus | 0:617334d8e3bb | 96 | // MOTOR DRIVERS - FEED |
Issus | 0:617334d8e3bb | 97 | HBridge feedMotor1(PIN_H1_IN1,PIN_H1_IN2); |
Issus | 0:617334d8e3bb | 98 | HBridge feedMotor0(PIN_H1_IN3,PIN_H1_IN4); |
Issus | 0:617334d8e3bb | 99 | |
Issus | 0:617334d8e3bb | 100 | // MOTOR DRIVERS - TAPE |
Issus | 0:617334d8e3bb | 101 | HBridge tapeMotor0(PIN_H2_IN2, PIN_H2_IN1); |
Issus | 0:617334d8e3bb | 102 | HBridge tapeMotor1(PIN_H2_IN4, PIN_H2_IN3); |
Issus | 0:617334d8e3bb | 103 | |
Issus | 0:617334d8e3bb | 104 | // GATES |
Issus | 0:617334d8e3bb | 105 | InterruptIn LN0_FEEDGATE(PIN_GATE_0); |
Issus | 0:617334d8e3bb | 106 | InterruptIn LN1_FEEDGATE(PIN_GATE_1); |
Issus | 0:617334d8e3bb | 107 | InterruptIn LN0_TAPEGATE(PIN_GATE_2); |
Issus | 0:617334d8e3bb | 108 | InterruptIn LN1_TAPEGATE(PIN_GATE_3); |
Issus | 0:617334d8e3bb | 109 | |
Issus | 0:617334d8e3bb | 110 | // FEEDER END BUTTONS |
Issus | 0:617334d8e3bb | 111 | InterruptIn BUTTON_GREEN_0(PIN_BTN_GREEN_0); |
Issus | 0:617334d8e3bb | 112 | InterruptIn BUTTON_GREEN_1(PIN_BTN_GREEN_1); |
Issus | 0:617334d8e3bb | 113 | |
Issus | 0:617334d8e3bb | 114 | InterruptIn BUTTON_YELLOW_0(PIN_LED_YELLOW_0); |
Issus | 0:617334d8e3bb | 115 | InterruptIn BUTTON_YELLOW_1(PIN_LED_YELLOW_1); |
Issus | 0:617334d8e3bb | 116 | |
Issus | 0:617334d8e3bb | 117 | InterruptIn BUTTON_BOARD_FEED(PIN_BOARD_FEED); |
Issus | 0:617334d8e3bb | 118 | |
Issus | 0:617334d8e3bb | 119 | // FEEDER END LEDS |
Issus | 0:617334d8e3bb | 120 | DigitalOut LED_0(PIN_BTN_LED_0); |
Issus | 0:617334d8e3bb | 121 | DigitalOut LED_1(PIN_BTN_LED_1); |
Issus | 0:617334d8e3bb | 122 | |
Issus | 0:617334d8e3bb | 123 | // DIGITAL CONTROL INPUT |
Issus | 0:617334d8e3bb | 124 | InterruptIn LANEIN_0(PIN_LANECTL_0); |
Issus | 0:617334d8e3bb | 125 | InterruptIn LANEIN_1(PIN_LANECTL_1); |
Issus | 0:617334d8e3bb | 126 | |
Issus | 0:617334d8e3bb | 127 | |
Issus | 0:617334d8e3bb | 128 | |
Issus | 0:617334d8e3bb | 129 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 130 | **** TAPE SLACK PICKUP |
Issus | 0:617334d8e3bb | 131 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 132 | |
Issus | 0:617334d8e3bb | 133 | void stopTapeL0() |
Issus | 0:617334d8e3bb | 134 | { |
Issus | 1:4d3738338cf1 | 135 | tape0PullTime.stop(); |
Issus | 1:4d3738338cf1 | 136 | tape0Takeup = 0; |
Issus | 1:4d3738338cf1 | 137 | tapeMotor0.Coast(); |
Issus | 0:617334d8e3bb | 138 | } |
Issus | 0:617334d8e3bb | 139 | |
Issus | 0:617334d8e3bb | 140 | |
Issus | 0:617334d8e3bb | 141 | void stopTapeL1() |
Issus | 0:617334d8e3bb | 142 | { |
Issus | 1:4d3738338cf1 | 143 | tape1PullTime.stop(); |
Issus | 1:4d3738338cf1 | 144 | tape1Takeup = 0; |
Issus | 1:4d3738338cf1 | 145 | tapeMotor1.Coast(); |
Issus | 0:617334d8e3bb | 146 | } |
Issus | 0:617334d8e3bb | 147 | |
Issus | 0:617334d8e3bb | 148 | |
Issus | 0:617334d8e3bb | 149 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 150 | **** BUTTON INTERRUPTS |
Issus | 0:617334d8e3bb | 151 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 152 | |
Issus | 0:617334d8e3bb | 153 | void button_yellow_0_int() |
Issus | 0:617334d8e3bb | 154 | { |
Issus | 0:617334d8e3bb | 155 | pc.printf("Yellow Button 0 Press\r\n"); |
Issus | 0:617334d8e3bb | 156 | } |
Issus | 0:617334d8e3bb | 157 | |
Issus | 0:617334d8e3bb | 158 | void button_yellow_1_int() |
Issus | 0:617334d8e3bb | 159 | { |
Issus | 0:617334d8e3bb | 160 | pc.printf("Yellow Button 1 Press\r\n"); |
Issus | 0:617334d8e3bb | 161 | } |
Issus | 0:617334d8e3bb | 162 | |
Issus | 0:617334d8e3bb | 163 | |
Issus | 0:617334d8e3bb | 164 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 165 | **** LANE 0 FEED CONTROL |
Issus | 0:617334d8e3bb | 166 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 167 | |
Issus | 0:617334d8e3bb | 168 | void setLane0Picking() |
Issus | 0:617334d8e3bb | 169 | { |
Issus | 0:617334d8e3bb | 170 | lane0State = PICKING; |
Issus | 0:617334d8e3bb | 171 | |
Issus | 0:617334d8e3bb | 172 | //todo: PwmOut |
Issus | 0:617334d8e3bb | 173 | SOLENOID_0 = 1; |
Issus | 0:617334d8e3bb | 174 | } |
Issus | 0:617334d8e3bb | 175 | |
Issus | 0:617334d8e3bb | 176 | void setLane0Feeding() |
Issus | 0:617334d8e3bb | 177 | { |
Issus | 0:617334d8e3bb | 178 | lane0State = FEEDING; |
Issus | 0:617334d8e3bb | 179 | lane0FeedEdge = 0; |
Issus | 0:617334d8e3bb | 180 | lane0LastEdge = LN0_FEEDGATE; |
Issus | 0:617334d8e3bb | 181 | |
Issus | 0:617334d8e3bb | 182 | // solenoid release |
Issus | 0:617334d8e3bb | 183 | SOLENOID_0 = 0; |
Issus | 0:617334d8e3bb | 184 | |
Issus | 0:617334d8e3bb | 185 | lane0Feed = 1; |
Issus | 0:617334d8e3bb | 186 | // motor on |
Issus | 0:617334d8e3bb | 187 | feedMotor0.Forward(); |
Issus | 0:617334d8e3bb | 188 | } |
Issus | 0:617334d8e3bb | 189 | |
Issus | 0:617334d8e3bb | 190 | void setLane0Reversing() |
Issus | 0:617334d8e3bb | 191 | { |
Issus | 0:617334d8e3bb | 192 | feedMotor0.Brake(); // yes.. this is only going to brake for a few dozen uS... |
Issus | 0:617334d8e3bb | 193 | |
Issus | 0:617334d8e3bb | 194 | lane0State = REVERSING; |
Issus | 0:617334d8e3bb | 195 | lane0FeedEdge = 0; |
Issus | 0:617334d8e3bb | 196 | lane0LastEdge = LN0_FEEDGATE; |
Issus | 0:617334d8e3bb | 197 | |
Issus | 0:617334d8e3bb | 198 | // go backwards till we smack the stop |
Issus | 0:617334d8e3bb | 199 | feedMotor0.Reverse(); |
Issus | 0:617334d8e3bb | 200 | } |
Issus | 0:617334d8e3bb | 201 | |
Issus | 0:617334d8e3bb | 202 | |
Issus | 0:617334d8e3bb | 203 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 204 | **** LANE 1 FEED CONTROL |
Issus | 0:617334d8e3bb | 205 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 206 | |
Issus | 0:617334d8e3bb | 207 | void setLane1Picking() |
Issus | 0:617334d8e3bb | 208 | { |
Issus | 0:617334d8e3bb | 209 | lane1State = PICKING; |
Issus | 0:617334d8e3bb | 210 | |
Issus | 0:617334d8e3bb | 211 | //todo: PwmOut |
Issus | 0:617334d8e3bb | 212 | SOLENOID_1 = 1; |
Issus | 0:617334d8e3bb | 213 | } |
Issus | 0:617334d8e3bb | 214 | |
Issus | 0:617334d8e3bb | 215 | |
Issus | 0:617334d8e3bb | 216 | void setLane1Feeding() |
Issus | 0:617334d8e3bb | 217 | { |
Issus | 0:617334d8e3bb | 218 | lane1State = FEEDING; |
Issus | 0:617334d8e3bb | 219 | lane1FeedEdge = 0; |
Issus | 0:617334d8e3bb | 220 | lane1LastEdge = LN0_FEEDGATE; |
Issus | 0:617334d8e3bb | 221 | |
Issus | 0:617334d8e3bb | 222 | // solenoid release |
Issus | 0:617334d8e3bb | 223 | SOLENOID_1 = 0; |
Issus | 0:617334d8e3bb | 224 | |
Issus | 0:617334d8e3bb | 225 | lane1Feed = 1; |
Issus | 0:617334d8e3bb | 226 | // motor on |
Issus | 0:617334d8e3bb | 227 | feedMotor1.Forward(); |
Issus | 0:617334d8e3bb | 228 | } |
Issus | 0:617334d8e3bb | 229 | |
Issus | 0:617334d8e3bb | 230 | void setLane1Reversing() |
Issus | 0:617334d8e3bb | 231 | { |
Issus | 0:617334d8e3bb | 232 | feedMotor1.Brake(); // yes.. this is only going to brake for a few dozen uS... |
Issus | 0:617334d8e3bb | 233 | |
Issus | 0:617334d8e3bb | 234 | lane1Feed = 0; |
Issus | 0:617334d8e3bb | 235 | |
Issus | 0:617334d8e3bb | 236 | lane1State = REVERSING; |
Issus | 0:617334d8e3bb | 237 | lane1FeedEdge = 0; |
Issus | 0:617334d8e3bb | 238 | lane1LastEdge = LN0_FEEDGATE; |
Issus | 0:617334d8e3bb | 239 | |
Issus | 0:617334d8e3bb | 240 | // go backwards till we smack the stop |
Issus | 0:617334d8e3bb | 241 | feedMotor1.Reverse(); |
Issus | 0:617334d8e3bb | 242 | } |
Issus | 0:617334d8e3bb | 243 | |
Issus | 0:617334d8e3bb | 244 | void coastLane0() |
Issus | 0:617334d8e3bb | 245 | { |
Issus | 0:617334d8e3bb | 246 | if (lane0State == IDLE) |
Issus | 0:617334d8e3bb | 247 | { |
Issus | 0:617334d8e3bb | 248 | feedMotor0.Coast(); |
Issus | 0:617334d8e3bb | 249 | } |
Issus | 0:617334d8e3bb | 250 | } |
Issus | 0:617334d8e3bb | 251 | |
Issus | 0:617334d8e3bb | 252 | void coastLane1() |
Issus | 0:617334d8e3bb | 253 | { |
Issus | 0:617334d8e3bb | 254 | if (lane1State == IDLE) |
Issus | 0:617334d8e3bb | 255 | { |
Issus | 0:617334d8e3bb | 256 | feedMotor1.Coast(); |
Issus | 0:617334d8e3bb | 257 | } |
Issus | 0:617334d8e3bb | 258 | } |
Issus | 0:617334d8e3bb | 259 | |
Issus | 0:617334d8e3bb | 260 | void stopLane0Reverse() |
Issus | 0:617334d8e3bb | 261 | { |
Issus | 0:617334d8e3bb | 262 | feedMotor0.Brake(); |
Issus | 0:617334d8e3bb | 263 | |
Issus | 1:4d3738338cf1 | 264 | lane0Feedout.attach(&coastLane0, 0.010); |
Issus | 0:617334d8e3bb | 265 | } |
Issus | 0:617334d8e3bb | 266 | |
Issus | 0:617334d8e3bb | 267 | void stopLane1Reverse() |
Issus | 0:617334d8e3bb | 268 | { |
Issus | 0:617334d8e3bb | 269 | feedMotor1.Brake(); |
Issus | 0:617334d8e3bb | 270 | |
Issus | 1:4d3738338cf1 | 271 | lane1Feedout.attach(&coastLane1, 0.010); |
Issus | 0:617334d8e3bb | 272 | } |
Issus | 0:617334d8e3bb | 273 | |
Issus | 0:617334d8e3bb | 274 | /***************************************************************************** |
Issus | 0:617334d8e3bb | 275 | **** MAIN |
Issus | 0:617334d8e3bb | 276 | *****************************************************************************/ |
Issus | 0:617334d8e3bb | 277 | |
Issus | 0:617334d8e3bb | 278 | |
Issus | 0:617334d8e3bb | 279 | int main() |
Issus | 0:617334d8e3bb | 280 | { |
Issus | 0:617334d8e3bb | 281 | // solenoids off |
Issus | 0:617334d8e3bb | 282 | SOLENOID_0 = 0; |
Issus | 0:617334d8e3bb | 283 | SOLENOID_1 = 0; |
Issus | 0:617334d8e3bb | 284 | |
Issus | 0:617334d8e3bb | 285 | // motors high impedance |
Issus | 0:617334d8e3bb | 286 | feedMotor0.Coast(); |
Issus | 0:617334d8e3bb | 287 | feedMotor1.Coast(); |
Issus | 0:617334d8e3bb | 288 | tapeMotor0.Coast(); |
Issus | 0:617334d8e3bb | 289 | tapeMotor1.Coast(); |
Issus | 0:617334d8e3bb | 290 | |
Issus | 0:617334d8e3bb | 291 | LED_0 = 1; |
Issus | 0:617334d8e3bb | 292 | LED_1 = 1; |
Issus | 0:617334d8e3bb | 293 | |
Issus | 0:617334d8e3bb | 294 | // give me time to get serial port connected |
Issus | 0:617334d8e3bb | 295 | wait(2); |
Issus | 0:617334d8e3bb | 296 | |
Issus | 0:617334d8e3bb | 297 | LED_0 = 0; |
Issus | 0:617334d8e3bb | 298 | LED_1 = 0; |
Issus | 0:617334d8e3bb | 299 | |
Issus | 1:4d3738338cf1 | 300 | // clear terminal screen - ignore compiler warning |
Issus | 0:617334d8e3bb | 301 | pc.printf("\x1Bc\x1B[2J"); |
Issus | 0:617334d8e3bb | 302 | |
Issus | 0:617334d8e3bb | 303 | pc.printf("\r\nFeeder POST...\r\n\r\n"); |
Issus | 0:617334d8e3bb | 304 | |
Issus | 1:4d3738338cf1 | 305 | UID feederID = IAP_ReadUID(); |
Issus | 1:4d3738338cf1 | 306 | pc.printf("Feeder UID: {%#10x-%#10x-%#10x-%#10x }\r\n\r\n", feederID.word0, feederID.word1, feederID.word2, feederID.word3); |
Issus | 0:617334d8e3bb | 307 | |
Issus | 0:617334d8e3bb | 308 | pc.printf("Gate 0 (L0 Feed): %d\r\n", LN0_FEEDGATE.read()); |
Issus | 0:617334d8e3bb | 309 | pc.printf("Gate 1 (L1 Feed): %d\r\n", LN1_FEEDGATE.read()); |
Issus | 0:617334d8e3bb | 310 | pc.printf("Gate 2 (L0 Tape): %d\r\n", LN0_TAPEGATE.read()); |
Issus | 0:617334d8e3bb | 311 | pc.printf("Gate 3 (L1 Tape): %d\r\n", LN1_TAPEGATE.read()); |
Issus | 0:617334d8e3bb | 312 | pc.printf("\r\n---\r\n"); |
Issus | 0:617334d8e3bb | 313 | |
Issus | 0:617334d8e3bb | 314 | pc.printf("Green Button 0 (L0): %d\r\n", BUTTON_GREEN_0.read()); |
Issus | 0:617334d8e3bb | 315 | pc.printf("Green Button 1 (L1): %d\r\n", BUTTON_GREEN_1.read()); |
Issus | 0:617334d8e3bb | 316 | pc.printf("Yellow Button 0 (L0): %d\r\n", BUTTON_YELLOW_0.read()); |
Issus | 0:617334d8e3bb | 317 | pc.printf("Yellow Button 1 (L1): %d\r\n", BUTTON_YELLOW_1.read()); |
Issus | 0:617334d8e3bb | 318 | pc.printf("\r\n---\r\n"); |
Issus | 0:617334d8e3bb | 319 | |
Issus | 0:617334d8e3bb | 320 | pc.printf("Lane 0 Control: %d\r\n", LANEIN_0.read()); |
Issus | 0:617334d8e3bb | 321 | pc.printf("Lane 1 Control: %d\r\n", LANEIN_0.read()); |
Issus | 0:617334d8e3bb | 322 | pc.printf("\r\n---\r\n"); |
Issus | 0:617334d8e3bb | 323 | pc.printf("END\r\n\r\n"); |
Issus | 0:617334d8e3bb | 324 | |
Issus | 0:617334d8e3bb | 325 | // make sure motors are at their end stops |
Issus | 0:617334d8e3bb | 326 | feedMotor0.Reverse(); |
Issus | 0:617334d8e3bb | 327 | wait_ms(50); |
Issus | 0:617334d8e3bb | 328 | feedMotor0.Coast(); |
Issus | 0:617334d8e3bb | 329 | |
Issus | 0:617334d8e3bb | 330 | // one after another, stall current is ~750mA per motor |
Issus | 0:617334d8e3bb | 331 | feedMotor1.Reverse(); |
Issus | 0:617334d8e3bb | 332 | wait_ms(50); |
Issus | 0:617334d8e3bb | 333 | feedMotor1.Coast(); |
Issus | 0:617334d8e3bb | 334 | |
Issus | 0:617334d8e3bb | 335 | // cover tape pickup interrupts |
Issus | 0:617334d8e3bb | 336 | /* Moved to main loop |
Issus | 0:617334d8e3bb | 337 | LN0_TAPEGATE.rise(&L0_Tape_int); |
Issus | 0:617334d8e3bb | 338 | LN0_TAPEGATE.fall(&L0_Tape_int); |
Issus | 0:617334d8e3bb | 339 | LN1_TAPEGATE.rise(&L1_Tape_int); |
Issus | 0:617334d8e3bb | 340 | LN1_TAPEGATE.fall(&L1_Tape_int); |
Issus | 0:617334d8e3bb | 341 | */ |
Issus | 0:617334d8e3bb | 342 | |
Issus | 0:617334d8e3bb | 343 | // button/gate state temp |
Issus | 0:617334d8e3bb | 344 | int b0 = 0; |
Issus | 0:617334d8e3bb | 345 | int b1 = 0; |
Issus | 0:617334d8e3bb | 346 | uint8_t c = 0; |
Issus | 0:617334d8e3bb | 347 | |
Issus | 0:617334d8e3bb | 348 | // timer for button debouncing. oldass hardware buttons are bouncy! |
Issus | 0:617334d8e3bb | 349 | Timer button0Time; |
Issus | 0:617334d8e3bb | 350 | Timer button1Time; |
Issus | 1:4d3738338cf1 | 351 | float t0 = 0; |
Issus | 1:4d3738338cf1 | 352 | float t1 = 0; |
Issus | 0:617334d8e3bb | 353 | |
Issus | 0:617334d8e3bb | 354 | while(true) |
Issus | 0:617334d8e3bb | 355 | { |
Issus | 0:617334d8e3bb | 356 | /********************************************************************** |
Issus | 1:4d3738338cf1 | 357 | ** PC USB COMMS - 2.01uS |
Issus | 0:617334d8e3bb | 358 | **********************************************************************/ |
Issus | 1:4d3738338cf1 | 359 | // no pc comms checking when feeding, no need |
Issus | 1:4d3738338cf1 | 360 | if ((lane0State == IDLE || lane0State == PICKING) && (lane1State == IDLE || lane1State == PICKING)) |
Issus | 0:617334d8e3bb | 361 | { |
Issus | 0:617334d8e3bb | 362 | while (pc.readable()) |
Issus | 0:617334d8e3bb | 363 | { |
Issus | 0:617334d8e3bb | 364 | c = pc.getc(); |
Issus | 0:617334d8e3bb | 365 | pc.putc(c); |
Issus | 0:617334d8e3bb | 366 | |
Issus | 0:617334d8e3bb | 367 | switch (pcCommand) |
Issus | 0:617334d8e3bb | 368 | { |
Issus | 0:617334d8e3bb | 369 | case READYFORCOMMAND: |
Issus | 0:617334d8e3bb | 370 | if (c == 'F') |
Issus | 0:617334d8e3bb | 371 | { |
Issus | 0:617334d8e3bb | 372 | pcCommand = FEED; |
Issus | 0:617334d8e3bb | 373 | pc.printf("\r\nFeed: "); |
Issus | 0:617334d8e3bb | 374 | } |
Issus | 0:617334d8e3bb | 375 | else if (c == 'P') |
Issus | 0:617334d8e3bb | 376 | { |
Issus | 0:617334d8e3bb | 377 | pcCommand = PICK; |
Issus | 0:617334d8e3bb | 378 | pc.printf("\r\nPick: "); |
Issus | 0:617334d8e3bb | 379 | } |
Issus | 0:617334d8e3bb | 380 | else if (c == 'E') |
Issus | 0:617334d8e3bb | 381 | { |
Issus | 0:617334d8e3bb | 382 | pcCommand = CLOSE; |
Issus | 0:617334d8e3bb | 383 | pc.printf("\r\nClose: "); |
Issus | 0:617334d8e3bb | 384 | } |
Issus | 0:617334d8e3bb | 385 | else if (c == 'C') |
Issus | 0:617334d8e3bb | 386 | { |
Issus | 0:617334d8e3bb | 387 | pcCommand = CONFIG; |
Issus | 0:617334d8e3bb | 388 | pc.printf("\r\nConfig: "); |
Issus | 0:617334d8e3bb | 389 | } |
Issus | 0:617334d8e3bb | 390 | break; |
Issus | 0:617334d8e3bb | 391 | case FEED: |
Issus | 0:617334d8e3bb | 392 | if (c == '0') |
Issus | 0:617334d8e3bb | 393 | { |
Issus | 0:617334d8e3bb | 394 | pc.printf("\rLane 0 Feeding\r\n"); |
Issus | 0:617334d8e3bb | 395 | setLane0Feeding(); |
Issus | 0:617334d8e3bb | 396 | } |
Issus | 0:617334d8e3bb | 397 | else if (c == '1') |
Issus | 0:617334d8e3bb | 398 | { |
Issus | 0:617334d8e3bb | 399 | pc.printf("\rLane 1 Feeding\r\n"); |
Issus | 0:617334d8e3bb | 400 | setLane1Feeding(); |
Issus | 0:617334d8e3bb | 401 | } |
Issus | 0:617334d8e3bb | 402 | |
Issus | 0:617334d8e3bb | 403 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 404 | break; |
Issus | 0:617334d8e3bb | 405 | case PICK: |
Issus | 0:617334d8e3bb | 406 | if (c == '0') |
Issus | 0:617334d8e3bb | 407 | { |
Issus | 0:617334d8e3bb | 408 | pc.printf("\rLane 0 Picking\r\n"); |
Issus | 0:617334d8e3bb | 409 | |
Issus | 0:617334d8e3bb | 410 | setLane0Picking(); |
Issus | 0:617334d8e3bb | 411 | } |
Issus | 0:617334d8e3bb | 412 | else if (c == '1') |
Issus | 0:617334d8e3bb | 413 | { |
Issus | 0:617334d8e3bb | 414 | pc.printf("\rLane 1 Picking\r\n"); |
Issus | 0:617334d8e3bb | 415 | |
Issus | 0:617334d8e3bb | 416 | setLane1Picking(); |
Issus | 0:617334d8e3bb | 417 | } |
Issus | 0:617334d8e3bb | 418 | |
Issus | 0:617334d8e3bb | 419 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 420 | break; |
Issus | 0:617334d8e3bb | 421 | case CLOSE: |
Issus | 0:617334d8e3bb | 422 | if (c == '0') |
Issus | 0:617334d8e3bb | 423 | { |
Issus | 0:617334d8e3bb | 424 | pc.printf("\rLane 0 Closing\r\n"); |
Issus | 0:617334d8e3bb | 425 | |
Issus | 0:617334d8e3bb | 426 | SOLENOID_0 = 0; |
Issus | 0:617334d8e3bb | 427 | lane0State = IDLE; |
Issus | 0:617334d8e3bb | 428 | } |
Issus | 0:617334d8e3bb | 429 | else if (c == '1') |
Issus | 0:617334d8e3bb | 430 | { |
Issus | 0:617334d8e3bb | 431 | pc.printf("\rLane 1 Closing\r\n"); |
Issus | 0:617334d8e3bb | 432 | |
Issus | 0:617334d8e3bb | 433 | SOLENOID_1 = 0; |
Issus | 0:617334d8e3bb | 434 | lane1State = IDLE; |
Issus | 0:617334d8e3bb | 435 | } |
Issus | 0:617334d8e3bb | 436 | |
Issus | 0:617334d8e3bb | 437 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 438 | break; |
Issus | 0:617334d8e3bb | 439 | case CONFIG: |
Issus | 0:617334d8e3bb | 440 | pcCommandPos++; |
Issus | 0:617334d8e3bb | 441 | if (pcConfig == NONE) |
Issus | 0:617334d8e3bb | 442 | { |
Issus | 0:617334d8e3bb | 443 | if (c == 'L') |
Issus | 0:617334d8e3bb | 444 | pcConfig = LANE; |
Issus | 0:617334d8e3bb | 445 | else |
Issus | 0:617334d8e3bb | 446 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 447 | } |
Issus | 0:617334d8e3bb | 448 | else if (pcConfig == LANE && selectedLane == UNSELECTED) |
Issus | 0:617334d8e3bb | 449 | { |
Issus | 0:617334d8e3bb | 450 | if (c == '0') |
Issus | 0:617334d8e3bb | 451 | selectedLane = ZERO; |
Issus | 0:617334d8e3bb | 452 | else if (c == '1') |
Issus | 0:617334d8e3bb | 453 | selectedLane = ONE; |
Issus | 0:617334d8e3bb | 454 | else |
Issus | 0:617334d8e3bb | 455 | { |
Issus | 0:617334d8e3bb | 456 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 457 | pcConfig = NONE; |
Issus | 0:617334d8e3bb | 458 | } |
Issus | 0:617334d8e3bb | 459 | } |
Issus | 0:617334d8e3bb | 460 | else if (pcConfig == LANE) |
Issus | 0:617334d8e3bb | 461 | { |
Issus | 0:617334d8e3bb | 462 | if (selectedLane == ZERO) |
Issus | 0:617334d8e3bb | 463 | lane0FeedDistance = c == '1' ? 1 : 2; |
Issus | 0:617334d8e3bb | 464 | else |
Issus | 0:617334d8e3bb | 465 | lane1FeedDistance = c == '1' ? 1 : 2; |
Issus | 0:617334d8e3bb | 466 | |
Issus | 0:617334d8e3bb | 467 | selectedLane = UNSELECTED; |
Issus | 0:617334d8e3bb | 468 | pcCommand = READYFORCOMMAND; |
Issus | 0:617334d8e3bb | 469 | pcConfig = NONE; |
Issus | 0:617334d8e3bb | 470 | } |
Issus | 0:617334d8e3bb | 471 | break; |
Issus | 0:617334d8e3bb | 472 | default: |
Issus | 0:617334d8e3bb | 473 | break; |
Issus | 0:617334d8e3bb | 474 | } |
Issus | 0:617334d8e3bb | 475 | } |
Issus | 0:617334d8e3bb | 476 | } |
Issus | 0:617334d8e3bb | 477 | |
Issus | 0:617334d8e3bb | 478 | /********************************************************************** |
Issus | 1:4d3738338cf1 | 479 | ** BUTTON CHECK - 3.65uS |
Issus | 0:617334d8e3bb | 480 | **********************************************************************/ |
Issus | 0:617334d8e3bb | 481 | if (lane0State != FEEDING) |
Issus | 0:617334d8e3bb | 482 | { |
Issus | 0:617334d8e3bb | 483 | b0 = BUTTON_GREEN_0; // read states |
Issus | 0:617334d8e3bb | 484 | b1 = BUTTON_GREEN_1; |
Issus | 0:617334d8e3bb | 485 | |
Issus | 0:617334d8e3bb | 486 | // BUTTON 0 |
Issus | 0:617334d8e3bb | 487 | if (b0 == button0LastState && b0) // button0 is pressed and was pressed last time too |
Issus | 0:617334d8e3bb | 488 | { |
Issus | 0:617334d8e3bb | 489 | if (lane0State == IDLE) |
Issus | 0:617334d8e3bb | 490 | { |
Issus | 0:617334d8e3bb | 491 | if (button0Time.read_ms() > 100) // wait for button state to be stable for 100ms |
Issus | 0:617334d8e3bb | 492 | { |
Issus | 0:617334d8e3bb | 493 | button0Time.reset(); |
Issus | 0:617334d8e3bb | 494 | button0Time.stop(); // no need to keep counting |
Issus | 0:617334d8e3bb | 495 | |
Issus | 0:617334d8e3bb | 496 | pc.printf("Picking 0\r\n"); |
Issus | 0:617334d8e3bb | 497 | setLane0Picking(); // open solenoid while button is down |
Issus | 0:617334d8e3bb | 498 | } |
Issus | 0:617334d8e3bb | 499 | } |
Issus | 0:617334d8e3bb | 500 | } |
Issus | 0:617334d8e3bb | 501 | else if (!b0 && b0 != button0LastState) // low transition - button released |
Issus | 0:617334d8e3bb | 502 | { |
Issus | 0:617334d8e3bb | 503 | button0LastState = b0; |
Issus | 0:617334d8e3bb | 504 | |
Issus | 0:617334d8e3bb | 505 | if (lane0State == PICKING) // not just a bounce |
Issus | 0:617334d8e3bb | 506 | { |
Issus | 0:617334d8e3bb | 507 | pc.printf("Feeding 0\r\n"); |
Issus | 0:617334d8e3bb | 508 | setLane0Feeding(); // button has been released, feed one component |
Issus | 0:617334d8e3bb | 509 | } |
Issus | 0:617334d8e3bb | 510 | } |
Issus | 0:617334d8e3bb | 511 | else // high transition |
Issus | 0:617334d8e3bb | 512 | { |
Issus | 0:617334d8e3bb | 513 | if (b0) // button pressed |
Issus | 0:617334d8e3bb | 514 | { |
Issus | 0:617334d8e3bb | 515 | button0Time.reset(); |
Issus | 0:617334d8e3bb | 516 | button0Time.start(); |
Issus | 0:617334d8e3bb | 517 | } |
Issus | 0:617334d8e3bb | 518 | } |
Issus | 0:617334d8e3bb | 519 | |
Issus | 0:617334d8e3bb | 520 | button0LastState = b0; |
Issus | 0:617334d8e3bb | 521 | } |
Issus | 0:617334d8e3bb | 522 | |
Issus | 0:617334d8e3bb | 523 | if (lane1State != FEEDING) |
Issus | 0:617334d8e3bb | 524 | { |
Issus | 0:617334d8e3bb | 525 | // BUTTON 1 |
Issus | 0:617334d8e3bb | 526 | if (b1 == button1LastState && b1) // button0 is pressed and was pressed last time too |
Issus | 0:617334d8e3bb | 527 | { |
Issus | 0:617334d8e3bb | 528 | if (lane1State == IDLE) |
Issus | 0:617334d8e3bb | 529 | { |
Issus | 0:617334d8e3bb | 530 | if (button1Time.read_ms() > 100) |
Issus | 0:617334d8e3bb | 531 | { |
Issus | 0:617334d8e3bb | 532 | button1Time.reset(); |
Issus | 0:617334d8e3bb | 533 | button1Time.stop(); |
Issus | 0:617334d8e3bb | 534 | |
Issus | 0:617334d8e3bb | 535 | pc.printf("Picking 1\r\n"); |
Issus | 0:617334d8e3bb | 536 | setLane1Picking(); |
Issus | 0:617334d8e3bb | 537 | } |
Issus | 0:617334d8e3bb | 538 | } |
Issus | 0:617334d8e3bb | 539 | } |
Issus | 0:617334d8e3bb | 540 | else if (!b1 && b1 != button1LastState) // low transition - button released |
Issus | 0:617334d8e3bb | 541 | { |
Issus | 0:617334d8e3bb | 542 | button1LastState = b1; |
Issus | 0:617334d8e3bb | 543 | |
Issus | 0:617334d8e3bb | 544 | if (lane1State == PICKING) |
Issus | 0:617334d8e3bb | 545 | { |
Issus | 0:617334d8e3bb | 546 | pc.printf("Feeding 1\r\n"); |
Issus | 0:617334d8e3bb | 547 | setLane1Feeding(); |
Issus | 0:617334d8e3bb | 548 | } |
Issus | 0:617334d8e3bb | 549 | } |
Issus | 0:617334d8e3bb | 550 | else // high transition |
Issus | 0:617334d8e3bb | 551 | { |
Issus | 0:617334d8e3bb | 552 | if (b1) |
Issus | 0:617334d8e3bb | 553 | { |
Issus | 0:617334d8e3bb | 554 | button1Time.reset(); |
Issus | 0:617334d8e3bb | 555 | button1Time.start(); |
Issus | 0:617334d8e3bb | 556 | } |
Issus | 0:617334d8e3bb | 557 | button1LastState = b1; |
Issus | 0:617334d8e3bb | 558 | } |
Issus | 0:617334d8e3bb | 559 | } |
Issus | 0:617334d8e3bb | 560 | |
Issus | 0:617334d8e3bb | 561 | /********************************************************************** |
Issus | 1:4d3738338cf1 | 562 | ** ENCODER CHECK - 3.38uS |
Issus | 0:617334d8e3bb | 563 | **********************************************************************/ |
Issus | 0:617334d8e3bb | 564 | |
Issus | 0:617334d8e3bb | 565 | b0 = LN0_FEEDGATE; |
Issus | 0:617334d8e3bb | 566 | b1 = LN1_FEEDGATE; |
Issus | 0:617334d8e3bb | 567 | |
Issus | 0:617334d8e3bb | 568 | // LANE 0 |
Issus | 0:617334d8e3bb | 569 | if (lane0State == FEEDING || lane0State == REVERSING) |
Issus | 0:617334d8e3bb | 570 | { |
Issus | 0:617334d8e3bb | 571 | if (lane0LastEdge != b0) |
Issus | 0:617334d8e3bb | 572 | { |
Issus | 0:617334d8e3bb | 573 | lane0FeedEdge++; |
Issus | 0:617334d8e3bb | 574 | } |
Issus | 0:617334d8e3bb | 575 | } |
Issus | 0:617334d8e3bb | 576 | lane0LastEdge = b0; |
Issus | 0:617334d8e3bb | 577 | |
Issus | 0:617334d8e3bb | 578 | // off stop, gap, past stop |
Issus | 0:617334d8e3bb | 579 | if (lane0State == FEEDING && |
Issus | 0:617334d8e3bb | 580 | lane0FeedEdge > (lane0FeedDistance * FEED_FWD_ENC_PULSE)) |
Issus | 0:617334d8e3bb | 581 | { |
Issus | 0:617334d8e3bb | 582 | lane0FeedEdge = 0; |
Issus | 0:617334d8e3bb | 583 | feedMotor0.Brake(); |
Issus | 0:617334d8e3bb | 584 | pc.printf("R0\r\n"); |
Issus | 0:617334d8e3bb | 585 | setLane0Reversing(); |
Issus | 0:617334d8e3bb | 586 | } |
Issus | 0:617334d8e3bb | 587 | // gap, return to stop |
Issus | 0:617334d8e3bb | 588 | else if (lane0State == REVERSING && lane0FeedEdge > 1) |
Issus | 0:617334d8e3bb | 589 | { |
Issus | 0:617334d8e3bb | 590 | lane0State = IDLE; |
Issus | 0:617334d8e3bb | 591 | lane0Feedout.attach(&stopLane0Reverse, 0.020); |
Issus | 0:617334d8e3bb | 592 | pc.printf("Idle 0\r\n"); |
Issus | 0:617334d8e3bb | 593 | } |
Issus | 0:617334d8e3bb | 594 | |
Issus | 0:617334d8e3bb | 595 | // LANE 1 |
Issus | 0:617334d8e3bb | 596 | if (lane1State == FEEDING || lane1State == REVERSING) |
Issus | 0:617334d8e3bb | 597 | { |
Issus | 0:617334d8e3bb | 598 | if (lane1LastEdge != b1) |
Issus | 0:617334d8e3bb | 599 | { |
Issus | 0:617334d8e3bb | 600 | lane1FeedEdge++; |
Issus | 0:617334d8e3bb | 601 | } |
Issus | 0:617334d8e3bb | 602 | } |
Issus | 0:617334d8e3bb | 603 | lane1LastEdge = b1; |
Issus | 0:617334d8e3bb | 604 | |
Issus | 0:617334d8e3bb | 605 | // off stop, gap, past stop |
Issus | 0:617334d8e3bb | 606 | if (lane1State == FEEDING && |
Issus | 0:617334d8e3bb | 607 | lane1FeedEdge > (lane1FeedDistance * FEED_FWD_ENC_PULSE)) |
Issus | 0:617334d8e3bb | 608 | { |
Issus | 0:617334d8e3bb | 609 | lane1FeedEdge = 0; |
Issus | 0:617334d8e3bb | 610 | feedMotor1.Brake(); |
Issus | 0:617334d8e3bb | 611 | pc.printf("R1\r\n"); |
Issus | 0:617334d8e3bb | 612 | setLane1Reversing(); |
Issus | 0:617334d8e3bb | 613 | } |
Issus | 0:617334d8e3bb | 614 | // gap, return to stop |
Issus | 0:617334d8e3bb | 615 | else if (lane1State == REVERSING && lane1FeedEdge > 1) |
Issus | 0:617334d8e3bb | 616 | { |
Issus | 0:617334d8e3bb | 617 | lane1State = IDLE; |
Issus | 0:617334d8e3bb | 618 | lane1Feedout.attach(&stopLane1Reverse, 0.020); |
Issus | 0:617334d8e3bb | 619 | pc.printf("Idle 1\r\n"); |
Issus | 0:617334d8e3bb | 620 | } |
Issus | 1:4d3738338cf1 | 621 | LED_0 = 1; |
Issus | 0:617334d8e3bb | 622 | |
Issus | 0:617334d8e3bb | 623 | /********************************************************************** |
Issus | 1:4d3738338cf1 | 624 | ** COVER TAPE CHECK - Gate closed: 41.98uS, Gate open 30.48uS |
Issus | 0:617334d8e3bb | 625 | **********************************************************************/ |
Issus | 0:617334d8e3bb | 626 | b0 = LN0_TAPEGATE; |
Issus | 0:617334d8e3bb | 627 | b1 = LN1_TAPEGATE; |
Issus | 1:4d3738338cf1 | 628 | if (tape0Takeup) |
Issus | 1:4d3738338cf1 | 629 | t0 = tape0PullTime.read(); // very slow.. |
Issus | 1:4d3738338cf1 | 630 | if (tape1Takeup) |
Issus | 1:4d3738338cf1 | 631 | t1 = tape0PullTime.read(); // very slow.. |
Issus | 0:617334d8e3bb | 632 | |
Issus | 0:617334d8e3bb | 633 | // LANE 0 |
Issus | 1:4d3738338cf1 | 634 | if (!b0 && !tape0Takeup) // cover tape slacked enough to interrupt gate |
Issus | 0:617334d8e3bb | 635 | { |
Issus | 1:4d3738338cf1 | 636 | tape0PullTime.reset(); |
Issus | 1:4d3738338cf1 | 637 | tape0PullTime.start(); |
Issus | 1:4d3738338cf1 | 638 | |
Issus | 0:617334d8e3bb | 639 | tape0Takeup = 1; |
Issus | 0:617334d8e3bb | 640 | tapeMotor0.Forward(); |
Issus | 0:617334d8e3bb | 641 | } |
Issus | 1:4d3738338cf1 | 642 | else if ((b0 && t0 > TAPE_MIN_PULL_TIME) || |
Issus | 1:4d3738338cf1 | 643 | (!b0 && tape0Takeup && t0 > TAPE_MAX_PULL_TIME)) |
Issus | 0:617334d8e3bb | 644 | { |
Issus | 0:617334d8e3bb | 645 | stopTapeL0(); |
Issus | 0:617334d8e3bb | 646 | } |
Issus | 0:617334d8e3bb | 647 | |
Issus | 0:617334d8e3bb | 648 | // LANE 1 |
Issus | 1:4d3738338cf1 | 649 | if (!b1 && !tape1Takeup) // gate opened |
Issus | 0:617334d8e3bb | 650 | { |
Issus | 1:4d3738338cf1 | 651 | tape1PullTime.reset(); |
Issus | 1:4d3738338cf1 | 652 | tape1PullTime.start(); |
Issus | 1:4d3738338cf1 | 653 | |
Issus | 0:617334d8e3bb | 654 | tape1Takeup = 1; |
Issus | 0:617334d8e3bb | 655 | tapeMotor1.Forward(); |
Issus | 0:617334d8e3bb | 656 | } |
Issus | 1:4d3738338cf1 | 657 | else if ((b1 && t1 > TAPE_MIN_PULL_TIME) || |
Issus | 1:4d3738338cf1 | 658 | (!b1 && tape1Takeup && t1 > TAPE_MAX_PULL_TIME)) |
Issus | 0:617334d8e3bb | 659 | { |
Issus | 0:617334d8e3bb | 660 | stopTapeL1(); |
Issus | 0:617334d8e3bb | 661 | } |
Issus | 0:617334d8e3bb | 662 | |
Issus | 0:617334d8e3bb | 663 | LED_0 = 0; // for timing check |
Issus | 0:617334d8e3bb | 664 | } |
Issus | 0:617334d8e3bb | 665 | } |