Mark Harris / Mbed 2 deprecated SIEMENS_DLFEEDER_LOOP

Loop based controller for dual lane siemens siplace feeder.

Dependencies:   USBDevice mbed

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?

UserRevisionLine numberNew 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 }