Mark Harris / Mbed 2 deprecated SIEMENS_DLFEEDER_LOOP

Loop based controller for dual lane siemens siplace feeder.

Dependencies:   USBDevice mbed

feederController.cpp@0:617334d8e3bb, 2017-02-02 (annotated)

Committer:
Issus
Date:
Thu Feb 02 19:18:12 2017 +0000
Revision:
0:617334d8e3bb
Child:
1:4d3738338cf1
Initial

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