Mark Harris
/
SIEMENS_DLFEEDER_LOOP
Loop based controller for dual lane siemens siplace feeder.
feederController.cpp
- Committer:
- Issus
- Date:
- 2017-02-04
- Revision:
- 1:4d3738338cf1
- Parent:
- 0:617334d8e3bb
File content as of revision 1:4d3738338cf1:
#include "mbed.h" #include "USBSerial.h" #include "HBridge.h" #include "feeder.h" #include "IAP_LPC11U.h" extern "C" void mbed_mac_address(char *); /************ TODO LIST: ************* * 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 * Store lane distance in EEPROM * Store retrievable aribitrary text for the lane in EEPROM (loaded part, openpnp part feeder name, whatever) *************/ // SETTINGS #define TAPE_MAX_PULL_TIME 3 // seconds #define TAPE_MIN_PULL_TIME 1 // seconds #define FEED_FWD_ENC_PULSE 2 // it's actually this value + 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; // 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; // TAPE PICKUP STOPPER Timer tape0PullTime; Timer tape1PullTime; /***************************************************************************** **** 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() { tape0PullTime.stop(); tape0Takeup = 0; tapeMotor0.Coast(); } void stopTapeL1() { tape1PullTime.stop(); 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.010); } void stopLane1Reverse() { feedMotor1.Brake(); lane1Feedout.attach(&coastLane1, 0.010); } /***************************************************************************** **** 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 - ignore compiler warning pc.printf("\x1Bc\x1B[2J"); pc.printf("\r\nFeeder POST...\r\n\r\n"); UID feederID = IAP_ReadUID(); pc.printf("Feeder UID: {%#10x-%#10x-%#10x-%#10x }\r\n\r\n", feederID.word0, feederID.word1, feederID.word2, feederID.word3); 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; float t0 = 0; float t1 = 0; while(true) { /********************************************************************** ** PC USB COMMS - 2.01uS **********************************************************************/ // no pc comms checking when feeding, no need if ((lane0State == IDLE || lane0State == PICKING) && (lane1State == IDLE || lane1State == PICKING)) { 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; } } } /********************************************************************** ** BUTTON CHECK - 3.65uS **********************************************************************/ 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"); setLane0Picking(); // open solenoid while button is down } } } else if (!b0 && b0 != button0LastState) // low transition - button released { button0LastState = b0; 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(); } } 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"); setLane1Picking(); } } } else if (!b1 && b1 != button1LastState) // low transition - button released { button1LastState = b1; if (lane1State == PICKING) { pc.printf("Feeding 1\r\n"); setLane1Feeding(); } } else // high transition { if (b1) { button1Time.reset(); button1Time.start(); } button1LastState = b1; } } /********************************************************************** ** ENCODER CHECK - 3.38uS **********************************************************************/ 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"); } LED_0 = 1; /********************************************************************** ** COVER TAPE CHECK - Gate closed: 41.98uS, Gate open 30.48uS **********************************************************************/ b0 = LN0_TAPEGATE; b1 = LN1_TAPEGATE; if (tape0Takeup) t0 = tape0PullTime.read(); // very slow.. if (tape1Takeup) t1 = tape0PullTime.read(); // very slow.. // LANE 0 if (!b0 && !tape0Takeup) // cover tape slacked enough to interrupt gate { tape0PullTime.reset(); tape0PullTime.start(); tape0Takeup = 1; tapeMotor0.Forward(); } else if ((b0 && t0 > TAPE_MIN_PULL_TIME) || (!b0 && tape0Takeup && t0 > TAPE_MAX_PULL_TIME)) { stopTapeL0(); } // LANE 1 if (!b1 && !tape1Takeup) // gate opened { tape1PullTime.reset(); tape1PullTime.start(); tape1Takeup = 1; tapeMotor1.Forward(); } else if ((b1 && t1 > TAPE_MIN_PULL_TIME) || (!b1 && tape1Takeup && t1 > TAPE_MAX_PULL_TIME)) { stopTapeL1(); } LED_0 = 0; // for timing check } }