Mark Harris
Loop based controller for dual lane siemens siplace feeder.
Diff: feederController.cpp
- Revision:
- 0:617334d8e3bb
- Child:
- 1:4d3738338cf1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/feederController.cpp Thu Feb 02 19:18:12 2017 +0000
@@ -0,0 +1,708 @@
+#include "mbed.h"
+#include "USBSerial.h"
+#include "HBridge.h"
+
+extern "C" void mbed_mac_address(char *);
+
+/************
+TODO LIST:
+*************
+* Move defs to .h file
+* Move tape pickup into main loop. Interrupts on it dont seem to be entirely reliable (spurrious)
+* printf needs to be replace with a function that putc instead for non-formatted strings
+* Convert lanes to a class so they there isn't duplicate code
+*************/
+
+// HARDWARE PIN DEFS
+#define PIN_LED P0_7
+#define PIN_VINREF P0_23
+
+#define PIN_LED_YELLOW_0 P0_9
+#define PIN_LED_YELLOW_1 P1_13
+
+#define PIN_BTN_GREEN_0 P0_22
+#define PIN_BTN_GREEN_1 P1_14
+
+#define PIN_BOARD_FEED P0_1
+
+#define PIN_GATE_0 P0_11
+#define PIN_GATE_1 P0_12
+#define PIN_GATE_2 P0_13
+#define PIN_GATE_3 P0_14
+
+#define PIN_LANECTL_0 P0_16
+#define PIN_LANECTL_1 P1_15
+
+#define PIN_RS485_DIR P0_17
+#define PIN_UART_TXD P0_18
+#define PIN_UART_RXD P0_19
+
+#define PIN_SOLCTL_0 P1_19
+#define PIN_SOLCTL_1 P1_25
+
+#define PIN_BTN_LED_0 P1_29
+#define PIN_BTN_LED_1 P1_22
+
+#define PIN_H1_IN1 P0_20
+#define PIN_H1_IN2 P0_2
+#define PIN_H1_IN3 P1_26
+#define PIN_H1_IN4 P1_27
+
+#define PIN_H2_IN1 P1_28
+#define PIN_H2_IN2 P1_31
+#define PIN_H2_IN3 P0_8
+#define PIN_H2_IN4 P1_21
+
+
+// SETTINGS
+#define TAPE_MAX_PULL_TIME 2 // seconds
+
+#define FEED_FWD_ENC_PULSE 2 // actually this + 1
+// encoder pulses: >(lane0FeedDistance * FEED_FWD_ENC_PULSE)
+// 3 for 1 position, 5 for 2 positions
+
+
+/*****************************************************************************
+**** GLOBALS
+*****************************************************************************/
+
+// SERIAL COMMAND INPUT
+enum command_t { READYFORCOMMAND, FEED, PICK, CLOSE, CONFIG };
+command_t pcCommand = READYFORCOMMAND;
+int pcCommandPos = 0;
+
+enum config_t { NONE, LANE };
+config_t pcConfig = NONE;
+
+enum lane_t { UNSELECTED, ZERO, ONE };
+lane_t selectedLane = UNSELECTED;
+
+// LANE STATES
+enum laneState_t { IDLE, PICKING, FEEDING, REVERSING };
+
+laneState_t lane0State = IDLE;
+laneState_t lane1State = IDLE;
+
+// LANE EDGE STATES
+int lane0LastEdge = 0;
+int lane1LastEdge = 0;
+
+// LANE EDGE COUNTERS
+int lane0FeedEdge = 0;
+int lane1FeedEdge = 0;
+
+// NUMBER OF POSITIONS TO FEED THROUGH
+int lane0FeedDistance = 1; // 1 = 2mm, 2 = 4mm
+int lane1FeedDistance = 1;
+
+// LOOP COUNT FOR BUTTONS (incase Timer is too slow)
+int button0PressCount = 0;
+int button1PressCount = 0;
+
+// FEEDER END BUTTON STATES
+int button0LastState = 0;
+int button1LastState = 0;
+
+// FEED STATE
+bool lane0Feed = 0;
+bool lane1Feed = 0;
+
+// PICKUP STATE
+bool tape0Takeup = 0;
+bool tape1Takeup = 0;
+
+// FEED STOPPER
+Timeout lane0Feedout;
+Timeout lane1Feedout;
+
+// PICKUP STOPPER
+Timeout lane0Pickupout;
+Timeout lane1Pickupout;
+
+/*****************************************************************************
+**** HARDWARE
+*****************************************************************************/
+
+// SOLENOIDS (TAPE COVER) - keep above usb to turn off quickly
+DigitalOut SOLENOID_0(PIN_SOLCTL_0, 0);
+DigitalOut SOLENOID_1(PIN_SOLCTL_1, 0);
+
+// USB VCOM UART for MBED Serial Port Driver
+USBSerial pc(0x1f00, 0x2012, 0x0001, false);
+
+// RS485
+//Serial control(PIN_UART_TXD, PIN_UART_RXD, 115200);
+DigitalOut RS485_DIR(PIN_RS485_DIR);
+
+// MOTOR DRIVERS - FEED
+HBridge feedMotor1(PIN_H1_IN1,PIN_H1_IN2);
+HBridge feedMotor0(PIN_H1_IN3,PIN_H1_IN4);
+
+// MOTOR DRIVERS - TAPE
+HBridge tapeMotor0(PIN_H2_IN2, PIN_H2_IN1);
+HBridge tapeMotor1(PIN_H2_IN4, PIN_H2_IN3);
+
+// GATES
+InterruptIn LN0_FEEDGATE(PIN_GATE_0);
+InterruptIn LN1_FEEDGATE(PIN_GATE_1);
+InterruptIn LN0_TAPEGATE(PIN_GATE_2);
+InterruptIn LN1_TAPEGATE(PIN_GATE_3);
+
+// FEEDER END BUTTONS
+InterruptIn BUTTON_GREEN_0(PIN_BTN_GREEN_0);
+InterruptIn BUTTON_GREEN_1(PIN_BTN_GREEN_1);
+
+InterruptIn BUTTON_YELLOW_0(PIN_LED_YELLOW_0);
+InterruptIn BUTTON_YELLOW_1(PIN_LED_YELLOW_1);
+
+InterruptIn BUTTON_BOARD_FEED(PIN_BOARD_FEED);
+
+// FEEDER END LEDS
+DigitalOut LED_0(PIN_BTN_LED_0);
+DigitalOut LED_1(PIN_BTN_LED_1);
+
+// DIGITAL CONTROL INPUT
+InterruptIn LANEIN_0(PIN_LANECTL_0);
+InterruptIn LANEIN_1(PIN_LANECTL_1);
+
+
+
+/*****************************************************************************
+**** TAPE SLACK PICKUP
+*****************************************************************************/
+
+void stopTapeL0()
+{
+ if (LN0_TAPEGATE)
+ {
+ tape0Takeup = 0;
+ tapeMotor0.Coast();
+ }
+}
+
+
+void stopTapeL1()
+{
+ if (LN1_TAPEGATE)
+ {
+ tape1Takeup = 0;
+ tapeMotor1.Coast();
+ }
+}
+
+
+/*****************************************************************************
+**** BUTTON INTERRUPTS
+*****************************************************************************/
+
+void button_yellow_0_int()
+{
+ pc.printf("Yellow Button 0 Press\r\n");
+}
+
+void button_yellow_1_int()
+{
+ pc.printf("Yellow Button 1 Press\r\n");
+}
+
+
+/*****************************************************************************
+**** LANE 0 FEED CONTROL
+*****************************************************************************/
+
+void setLane0Picking()
+{
+ lane0State = PICKING;
+
+ //todo: PwmOut
+ SOLENOID_0 = 1;
+}
+
+void setLane0Feeding()
+{
+ lane0State = FEEDING;
+ lane0FeedEdge = 0;
+ lane0LastEdge = LN0_FEEDGATE;
+
+ // solenoid release
+ SOLENOID_0 = 0;
+
+ lane0Feed = 1;
+ // motor on
+ feedMotor0.Forward();
+}
+
+void setLane0Reversing()
+{
+ feedMotor0.Brake(); // yes.. this is only going to brake for a few dozen uS...
+
+ lane0State = REVERSING;
+ lane0FeedEdge = 0;
+ lane0LastEdge = LN0_FEEDGATE;
+
+ // go backwards till we smack the stop
+ feedMotor0.Reverse();
+}
+
+
+/*****************************************************************************
+**** LANE 1 FEED CONTROL
+*****************************************************************************/
+
+void setLane1Picking()
+{
+ lane1State = PICKING;
+
+ //todo: PwmOut
+ SOLENOID_1 = 1;
+}
+
+
+void setLane1Feeding()
+{
+ lane1State = FEEDING;
+ lane1FeedEdge = 0;
+ lane1LastEdge = LN0_FEEDGATE;
+
+ // solenoid release
+ SOLENOID_1 = 0;
+
+ lane1Feed = 1;
+ // motor on
+ feedMotor1.Forward();
+}
+
+void setLane1Reversing()
+{
+ feedMotor1.Brake(); // yes.. this is only going to brake for a few dozen uS...
+
+ lane1Feed = 0;
+
+ lane1State = REVERSING;
+ lane1FeedEdge = 0;
+ lane1LastEdge = LN0_FEEDGATE;
+
+ // go backwards till we smack the stop
+ feedMotor1.Reverse();
+}
+
+void coastLane0()
+{
+ if (lane0State == IDLE)
+ {
+ feedMotor0.Coast();
+ }
+}
+
+void coastLane1()
+{
+ if (lane1State == IDLE)
+ {
+ feedMotor1.Coast();
+ }
+}
+
+void stopLane0Reverse()
+{
+ feedMotor0.Brake();
+
+ lane0Feedout.attach(&coastLane0, 0.020);
+}
+
+void stopLane1Reverse()
+{
+ feedMotor1.Brake();
+
+ lane1Feedout.attach(&coastLane1, 0.020);
+}
+
+/*****************************************************************************
+**** MAIN
+*****************************************************************************/
+
+
+int main()
+{
+ // solenoids off
+ SOLENOID_0 = 0;
+ SOLENOID_1 = 0;
+
+ // motors high impedance
+ feedMotor0.Coast();
+ feedMotor1.Coast();
+ tapeMotor0.Coast();
+ tapeMotor1.Coast();
+
+ LED_0 = 1;
+ LED_1 = 1;
+
+ // give me time to get serial port connected
+ wait(2);
+
+ LED_0 = 0;
+ LED_1 = 0;
+
+ // clear terminal screen
+ pc.printf("\x1Bc\x1B[2J");
+
+ pc.printf("\r\nFeeder POST...\r\n\r\n");
+
+ char mac[6];
+ mbed_mac_address(mac);
+ uint32_t devId = mac[3] << 16 | mac[4] << 8 | mac[5];
+ pc.printf("Feeder ID: %d [%#04x::%#04x::%#04x]\r\n\r\n", devId, mac[3], mac[4], mac[5]);
+
+ pc.printf("Gate 0 (L0 Feed): %d\r\n", LN0_FEEDGATE.read());
+ pc.printf("Gate 1 (L1 Feed): %d\r\n", LN1_FEEDGATE.read());
+ pc.printf("Gate 2 (L0 Tape): %d\r\n", LN0_TAPEGATE.read());
+ pc.printf("Gate 3 (L1 Tape): %d\r\n", LN1_TAPEGATE.read());
+ pc.printf("\r\n---\r\n");
+
+ pc.printf("Green Button 0 (L0): %d\r\n", BUTTON_GREEN_0.read());
+ pc.printf("Green Button 1 (L1): %d\r\n", BUTTON_GREEN_1.read());
+ pc.printf("Yellow Button 0 (L0): %d\r\n", BUTTON_YELLOW_0.read());
+ pc.printf("Yellow Button 1 (L1): %d\r\n", BUTTON_YELLOW_1.read());
+ pc.printf("\r\n---\r\n");
+
+ pc.printf("Lane 0 Control: %d\r\n", LANEIN_0.read());
+ pc.printf("Lane 1 Control: %d\r\n", LANEIN_0.read());
+ pc.printf("\r\n---\r\n");
+ pc.printf("END\r\n\r\n");
+
+ // make sure motors are at their end stops
+ feedMotor0.Reverse();
+ wait_ms(50);
+ feedMotor0.Coast();
+
+ // one after another, stall current is ~750mA per motor
+ feedMotor1.Reverse();
+ wait_ms(50);
+ feedMotor1.Coast();
+
+ // cover tape pickup interrupts
+ /* Moved to main loop
+ LN0_TAPEGATE.rise(&L0_Tape_int);
+ LN0_TAPEGATE.fall(&L0_Tape_int);
+ LN1_TAPEGATE.rise(&L1_Tape_int);
+ LN1_TAPEGATE.fall(&L1_Tape_int);
+ */
+
+ // button/gate state temp
+ int b0 = 0;
+ int b1 = 0;
+ uint8_t c = 0;
+
+ // timer for button debouncing. oldass hardware buttons are bouncy!
+ Timer button0Time;
+ Timer button1Time;
+
+ while(true)
+ {
+ /**********************************************************************
+ ** PC USB COMMS
+ **********************************************************************/
+ // no pc comms checking when feeding, can't afford the time
+ if (lane0State == IDLE && lane1State == IDLE)
+ {
+ while (pc.readable())
+ {
+ c = pc.getc();
+ pc.putc(c);
+
+ switch (pcCommand)
+ {
+ case READYFORCOMMAND:
+ if (c == 'F')
+ {
+ pcCommand = FEED;
+ pc.printf("\r\nFeed: ");
+ }
+ else if (c == 'P')
+ {
+ pcCommand = PICK;
+ pc.printf("\r\nPick: ");
+ }
+ else if (c == 'E')
+ {
+ pcCommand = CLOSE;
+ pc.printf("\r\nClose: ");
+ }
+ else if (c == 'C')
+ {
+ pcCommand = CONFIG;
+ pc.printf("\r\nConfig: ");
+ }
+ break;
+ case FEED:
+ if (c == '0')
+ {
+ pc.printf("\rLane 0 Feeding\r\n");
+ setLane0Feeding();
+ }
+ else if (c == '1')
+ {
+ pc.printf("\rLane 1 Feeding\r\n");
+ setLane1Feeding();
+ }
+
+ pcCommand = READYFORCOMMAND;
+ break;
+ case PICK:
+ if (c == '0')
+ {
+ pc.printf("\rLane 0 Picking\r\n");
+
+ setLane0Picking();
+ }
+ else if (c == '1')
+ {
+ pc.printf("\rLane 1 Picking\r\n");
+
+ setLane1Picking();
+ }
+
+ pcCommand = READYFORCOMMAND;
+ break;
+ case CLOSE:
+ if (c == '0')
+ {
+ pc.printf("\rLane 0 Closing\r\n");
+
+ SOLENOID_0 = 0;
+ lane0State = IDLE;
+ }
+ else if (c == '1')
+ {
+ pc.printf("\rLane 1 Closing\r\n");
+
+ SOLENOID_1 = 0;
+ lane1State = IDLE;
+ }
+
+ pcCommand = READYFORCOMMAND;
+ break;
+ case CONFIG:
+ pcCommandPos++;
+ if (pcConfig == NONE)
+ {
+ if (c == 'L')
+ pcConfig = LANE;
+ else
+ pcCommand = READYFORCOMMAND;
+ }
+ else if (pcConfig == LANE && selectedLane == UNSELECTED)
+ {
+ if (c == '0')
+ selectedLane = ZERO;
+ else if (c == '1')
+ selectedLane = ONE;
+ else
+ {
+ pcCommand = READYFORCOMMAND;
+ pcConfig = NONE;
+ }
+ }
+ else if (pcConfig == LANE)
+ {
+ if (selectedLane == ZERO)
+ lane0FeedDistance = c == '1' ? 1 : 2;
+ else
+ lane1FeedDistance = c == '1' ? 1 : 2;
+
+ selectedLane = UNSELECTED;
+ pcCommand = READYFORCOMMAND;
+ pcConfig = NONE;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+
+
+ LED_0 = 1; // for timing check with oscope/logic analyser
+
+ /**********************************************************************
+ ** BUTTON CHECK
+ **********************************************************************/
+ if (lane0State != FEEDING)
+ {
+ b0 = BUTTON_GREEN_0; // read states
+ b1 = BUTTON_GREEN_1;
+
+ // BUTTON 0
+ if (b0 == button0LastState && b0) // button0 is pressed and was pressed last time too
+ {
+ if (lane0State == IDLE)
+ {
+ if (button0Time.read_ms() > 100) // wait for button state to be stable for 100ms
+ {
+ button0Time.reset();
+ button0Time.stop(); // no need to keep counting
+
+ pc.printf("Picking 0\r\n");
+ button0PressCount = 0;
+ setLane0Picking(); // open solenoid while button is down
+ }
+ }
+ }
+ else if (!b0 && b0 != button0LastState) // low transition - button released
+ {
+ button0LastState = b0;
+ button0PressCount = 0;
+
+ if (lane0State == PICKING) // not just a bounce
+ {
+ pc.printf("Feeding 0\r\n");
+ setLane0Feeding(); // button has been released, feed one component
+ }
+ }
+ else // high transition
+ {
+ if (b0) // button pressed
+ {
+ button0Time.reset();
+ button0Time.start();
+ }
+ button0PressCount = 0;
+ }
+
+ button0LastState = b0;
+ }
+
+ if (lane1State != FEEDING)
+ {
+ // BUTTON 1
+ if (b1 == button1LastState && b1) // button0 is pressed and was pressed last time too
+ {
+ if (lane1State == IDLE)
+ {
+ if (button1Time.read_ms() > 100)
+ {
+ button1Time.reset();
+ button1Time.stop();
+
+ pc.printf("Picking 1\r\n");
+ button1PressCount = 0;
+ setLane1Picking();
+ }
+ }
+ }
+ else if (!b1 && b1 != button1LastState) // low transition - button released
+ {
+ button1LastState = b1;
+ button1PressCount = 0;
+
+ if (lane1State == PICKING)
+ {
+ pc.printf("Feeding 1\r\n");
+ setLane1Feeding();
+ }
+ }
+ else // high transition
+ {
+ if (b1)
+ {
+ button1Time.reset();
+ button1Time.start();
+ }
+ button1LastState = b1;
+ button1PressCount = 0;
+ }
+ }
+
+ /**********************************************************************
+ ** ENCODER CHECK
+ **********************************************************************/
+
+ b0 = LN0_FEEDGATE;
+ b1 = LN1_FEEDGATE;
+
+ // LANE 0
+ if (lane0State == FEEDING || lane0State == REVERSING)
+ {
+ if (lane0LastEdge != b0)
+ {
+ lane0FeedEdge++;
+ }
+ }
+ lane0LastEdge = b0;
+
+ // off stop, gap, past stop
+ if (lane0State == FEEDING &&
+ lane0FeedEdge > (lane0FeedDistance * FEED_FWD_ENC_PULSE))
+ {
+ lane0FeedEdge = 0;
+ feedMotor0.Brake();
+ pc.printf("R0\r\n");
+ setLane0Reversing();
+ }
+ // gap, return to stop
+ else if (lane0State == REVERSING && lane0FeedEdge > 1)
+ {
+ lane0State = IDLE;
+ lane0Feedout.attach(&stopLane0Reverse, 0.020);
+ pc.printf("Idle 0\r\n");
+ }
+
+ // LANE 1
+ if (lane1State == FEEDING || lane1State == REVERSING)
+ {
+ if (lane1LastEdge != b1)
+ {
+ lane1FeedEdge++;
+ }
+ }
+ lane1LastEdge = b1;
+
+ // off stop, gap, past stop
+ if (lane1State == FEEDING &&
+ lane1FeedEdge > (lane1FeedDistance * FEED_FWD_ENC_PULSE))
+ {
+ lane1FeedEdge = 0;
+ feedMotor1.Brake();
+ pc.printf("R1\r\n");
+ setLane1Reversing();
+ }
+ // gap, return to stop
+ else if (lane1State == REVERSING && lane1FeedEdge > 1)
+ {
+ lane1State = IDLE;
+ lane1Feedout.attach(&stopLane1Reverse, 0.020);
+ pc.printf("Idle 1\r\n");
+ }
+
+ /**********************************************************************
+ ** COVER TAPE CHECK
+ **********************************************************************/
+ b0 = LN0_TAPEGATE;
+ b1 = LN1_TAPEGATE;
+
+ // LANE 0
+ if (!b0 && !tape0Takeup)
+ {
+ tape0Takeup = 1;
+ tapeMotor0.Forward();
+ lane0Pickupout.attach(&stopTapeL0, TAPE_MAX_PULL_TIME);
+ }
+ else if (b0)
+ {
+ stopTapeL0();
+ }
+
+ // LANE 1
+ if (!b1 && !tape1Takeup)
+ {
+ tape1Takeup = 1;
+ tapeMotor1.Forward();
+ lane1Pickupout.attach(&stopTapeL1, TAPE_MAX_PULL_TIME);
+ }
+ else if (b1)
+ {
+ stopTapeL1();
+ }
+
+ LED_0 = 0; // for timing check
+ }
+}