File content as of revision 23:f9e7e64784be:

#include "mbed.h"
//#include "Colour.h"
#include "colourProcessing.h"

//Setup pins to FPGA
DigitalOut startSort(p5);       //A positive edge tells the FPGA to start sorting.
DigitalIn sortComplete(p16);
/*
  The command codes for the three sort select bits(ColourBit1 - ColourBit2)
  See datasheet for furter information
    000     Sort red chip
    001     Sort green chip
    010     Sort blue chip
    011     Bin the chip
    100     Recycle the chip
    101     Nothing
    110     Nothing
    111     Nothing
*/

DigitalOut colourBit1(p6);      //The 3 bits below are select bits for the sorter.
DigitalOut colourBit2(p7);
DigitalOut colourBit3(p8);
DigitalOut startDispense(p18);   //A positive edge tells the FPGA to start dispensing. p9
/*
    00      Dispense red chip
    01      Dispense green chip
    10      Dispense blue chip
    11      Nothing
*/

DigitalOut dispenseBit1(p19);  //The 2 bits below are select bits for the dispenser.p10
DigitalOut dispenseBit2(p20); //p11

DigitalIn dispenseComplete(p11);    //FPGA sets to 1 once complete until another signal is recieved.
DigitalOut select(p15); //0 for control, 1 for maintenance.


void setMaintenanceSelect(bool bit6, bool bit5, bool bit4, bool bit3, bool bit2, bool bit1)
{
    startSort = bit1;
    colourBit1 = bit2;
    colourBit2 = bit3;
    colourBit3 = bit4;
    startDispense = bit5;
    dispenseBit1 = bit6;
}

void setMaintenanceStart(bool value)
{
    dispenseBit2 = value;
}
void waitForMaintenanceComplete()
{
    while(!sortComplete) {
       
    }
   
}

void setDispenseSelect(bool dBit1, bool dBit2)
{
    dispenseBit1 = dBit1;
    dispenseBit2 = dBit2;
}

void setSortSelect(bool sBit1, bool sBit2, bool sBit3)
{
    colourBit1 = sBit1;
    colourBit2 = sBit2;
    colourBit3 = sBit3;
}

void maintain(StateMachine maintainState)
{
    select = 1; //setting FPGA to maintenance mode.
    setMaintenanceSelect(true,true,true,true,true,true);
    switch(maintainState) {
        case RB_LEFT:
            setMaintenanceSelect(false,false,false,true,false,true);
            break;
        case RB_CENTRE:
            setMaintenanceSelect(false,false,false,true,true,false);
            break;
        case RB_RIGHT:
            setMaintenanceSelect(false,false,false,true,true,true);
            break;
        case GO_UP:
            setMaintenanceSelect(false,false,true,false,false,false);
            break;
        case GO_CENTRE:
            setMaintenanceSelect(false,false,true,false,false,true);
            break;
        case GO_DOWN:
            setMaintenanceSelect(false,false,true,false,true,false);
            break;
        case BR_LEFT:
            setMaintenanceSelect(false,false,true,false,true,true);
            break;
        case BR_RIGHT:
            setMaintenanceSelect(false,false,true,true,false,false);
            break;
        case R_PUSH:
            setMaintenanceSelect(false,false,true,true,false,true);
            break;
        case R_HOME:
            setMaintenanceSelect(false,false,true,true,true,false);
            break;
        case GB_LEFT:
            setMaintenanceSelect(false,false,true,true,true,true);
            break;
        case GB_CENTRE:
            setMaintenanceSelect(false,true,false,false,false,false);
            break;
        case GB_RIGHT:
            setMaintenanceSelect(false,true,false,false,false,true);
            break;
        case maintenanceEND:
            setMaintenanceSelect(false,true,false,false,true,false);
            break;
        default:
            break;
    }
    setMaintenanceStart(true);
    waitForMaintenanceComplete();
    setMaintenanceStart(false);
    setMaintenanceSelect(true,false,true,true,true,false);
}

void waitForDispenseComplete()
{
    while(!dispenseComplete) {
        
    }
    
}

void dispense(Colour colour)
{
    startDispense = false;    
    bool validDispense = false;
    select = 0;
    setDispenseSelect(true,true);
    // A dispense operation can only take place if there are chips
   
    switch(colour) {
        case RED:
            if(redAmount > 0 || operationMode == false) {
                setDispenseSelect(false,false);
                redAmount--;
                validDispense = true;
            }
            break;
        case GREEN:
            if(greenAmount > 0 || operationMode == false) {
                setDispenseSelect(true,false);
                greenAmount--;
                validDispense = true;
            }
            break;
        case BLUE:
            if(blueAmount > 0 || operationMode == false) {
                setDispenseSelect(false,true);
                blueAmount--;
                validDispense = true;
            }
            break;
    }
    
    writeFile(redAmount,greenAmount,blueAmount,recycleAmount);
    if(validDispense || operationMode == false) {
        startDispense = true; //set the startDispense line high.
        waitForDispenseComplete();
        
        startDispense = false;
        setDispenseSelect(true,true);
    }
}
void dispenseOrder(int r, int g, int b)
{
//TODO: Check if there are enough chips of each colour to dispense the order.
    operationMode = false;
    for(int i = r; i > 0; i--) {
        dispense(RED);
    }
    for(int i = g; i > 0; i--) {
        dispense(GREEN);
    }
    for(int i = b; i > 0; i--) {
        dispense(BLUE);
    }
    operationMode = true;
}

void waitForSortComplete()
{
    while(!sortComplete) {}
}

void lift()
{
    select = 0; //Setting to operation mode just in case it has not been set.
    setSortSelect(true,true,true);
    setSortSelect(true,false,true);
    startSort = true; //set the startDispense line high.
    waitForSortComplete();
    startSort = false;
    setSortSelect(true,true,true);    
}



void recycle()
{
    select = 0; //Setting to operation mode just in case it has not been set.
    setSortSelect(true,true,true);
    setSortSelect(false,false,true);
    recycleAmount++;
    startSort = true; //set the startDispense line high.
    waitForSortComplete();
    startSort = false;
    setSortSelect(true,true,true);
    if(recycleAmount >= 5) {
        wait(1);
        lift();
        wait(1);
        recycleAmount = 0;
    }
}



void sort(Colour colour)
{
    if(colour == NONE) {
        return;
    } else {
        startSort = false;
        select = 0; //Setting to operation mode just in case it has not been set.
        setSortSelect(true,true,true);
        switch(colour) {
            case RED:
                if(redAmount >= tubeSize) {
                    recycle();
                    return;
                }
                setSortSelect(false,false,false);
                redAmount++;
                break;
            case GREEN:
                if(greenAmount >= tubeSize) {
                    recycle();
                    return;
                }
                setSortSelect(true,false,false);
                greenAmount++;
                break;
            case BLUE:
                if(blueAmount >= tubeSize) {
                    recycle();
                    return;
                }
                setSortSelect(false,true,false);
                blueAmount++;
                break;
            case BIN:
                setSortSelect(true,true,false);
                break;
        }
        startSort = true;
        waitForSortComplete();
        startSort = false;
        setSortSelect(true,true,true);
        writeFile(redAmount,greenAmount,blueAmount,recycleAmount);
        wait(0.3);
    }
}



void dispenseAll()
{
    dispenseOrder(redAmount,greenAmount,blueAmount);
    redAmount=0; blueAmount=0; greenAmount=0;
    writeFile(redAmount,greenAmount,blueAmount,recycleAmount);
}