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
+    }
+}