File content as of revision 0:617334d8e3bb:

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