File content as of revision 19:61b21ac4896e:

#include "mbed.h"
#include "WattBob_TextLCD.h"
#include "TCS3472_I2C.h"
#include "MCP23017.h"
#include <string>
#include <time.h>
//#include <future>
#include "globals.h"
#include "commander.h"
#include "fpga.h"

#define BACKLIGHT_ON(INTERFACE) INTERFACE->write_bit(1, 4);
#define BACKLIGHT_OFF(INTERFACE) INTERFACE->write_bit(0, 4);

#define LCDFL() lcd->locate(0,0);
#define LCDSL() lcd->locate(1,0);
#define D_LEDS_OFF() i2cport->write_bit(0, 12); i2cport->write_bit(0, 13); i2cport->write_bit(0, 14); i2cport->write_bit(0, 15);
#define U_LEDS_OFF() myLED1 = 0; myLED2 = 0; myLED3 = 0; myLED4 = 0;

DigitalOut myLED1(LED1);
DigitalOut myLED2(LED2);
DigitalOut myLED3(LED3);
DigitalOut myLED4(LED4);

MCP23017 *i2cport;
WattBob_TextLCD *lcd;

TCS3472_I2C rgbSensor(p28, p27);
Serial      pc(USBTX, USBRX);
uint8_t     rxBuffer[kSmallBufferSize + 1];
int 		rxIndex = 0;

Commander 	_commander = Commander();
Commander 	*commander = &_commander;
FPGA 		_fpga = FPGA();
extern FPGA		*fpga = &_fpga;

extern Block _HazBlock;
Block *HazBlock = &_HazBlock;

extern PCModes currentMode;

void initInternal();
void initPort(int baudRate=kDefaultBaudRate);
void printPCDetectedText();
bool displayAbortDialog();
void Rx_interrupt();
bool checkColour(int colourValues[]);
void runInServoTestMode();
void displayWaitingLine();
void displayPCStatus();
bool waitForBlock();
void sortBlock();

int main()
{
    initInternal();
    initPort();
    srand((unsigned)time(NULL));
    U_LEDS_OFF();
    lcd->cls();
    myLED1 = 1;

    rgbSensor.enablePowerAndRGBC();
    rgbSensor.setIntegrationTime(2.4);

    // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
    // https://developer.mbed.org/cookbook/Serial-Interrupts
    pc.attach(&Rx_interrupt, Serial::RxIrq);

    for (;;) {
        lcd->cls();
        i2cport->write_bit(1, 12);
        lcd->printf("1: Start sorting.");
        LCDSL();
        i2cport->write_bit(1,13);
        lcd->printf("2: Connect to PC");

        fpga->moveStoppingServo(Stop);
        fpga->moveSortingServo(NonHaz);

        int selection = 0;
        do {
            myLED4 = selection;
            selection = readSwitches();
        } while (selection != 1 && selection != 2 && connectedToPC == false);
        D_LEDS_OFF();
        if (selection == 1) {
            // User selected op 1: Start sorting autonomously.
            i2cport->write_bit(1, 12);
            lcd->cls();
            LCDFL();
            lcd->printf("Starting sorting");
            wait(0.5);
            D_LEDS_OFF();

            for(;;) {
                displayWaitingLine();
                lcd->printf("for block.");

                i2cport->write_bit(1, 15);

                // Break and return to main menu i.e. Start Op, or Connect to PC.
                if (waitForBlock() == false) {
                    D_LEDS_OFF();
                    break;
                } else {
                    sortBlock();
                }
            }
        }

        if (selection == 2 || connectedToPC == true) {
            for (;;) {
                displayPCStatus();

                i2cport->write_bit(1, 15);
                int abortOperation = false;
                while (connectedToPC == false && abortOperation == false) {
                    abortOperation = readSwitches() == 4;
                }

                if (abortOperation == true) {
                    D_LEDS_OFF();
                    break;
                }

                displayPCStatus();

                while (abortOperation == false && connectedToPC == true) {
                    if (currentMode == Maintanence) {
                        if (runServoTest == true)
                            runInServoTestMode();
                    } else if (currentMode == Normal) {
                        displayPCStatus();
                        while (currentMode == Normal) {
                            if (currentState == Pause) {
                                lcd->cls();
                                lcd->locate(0,0);
                                lcd->printf("Sorting Paused.");
                                lcd->locate(1,0);
                                lcd->printf("1: Start");
                                int button = 0;
                                while (currentState == Pause) {
                                    button = readSwitches();
                                    if (button == 1) {
                                        pc.printf("state:start\n");
                                        currentState = Start;
                                    } else if (button == 4) {
                                        goto setModeNone;
                                    }
                                }
                            }
                            if (currentState == Start) {
                                while (currentState == Start) {
                                    if (waitForBlock() == false) {
                                        currentState = Pause;
                                        if (connectedToPC == true) {
                                            // TODO: Tell PC to update UI if aborted from MBED.
                                            pc.printf("state:pause\n");

                                            continue;
                                        } else if (connectedToPC == false) {
                                            currentMode = None;
                                            abortOperation = true;
                                        }
                                    } else {
                                        sortBlock();
                                    }
                                }
                            }
                        }
                    } else if (currentMode == None) {
setModeNone:
                        displayPCStatus();
                        while (currentMode == None && abortOperation == false) {
                            abortOperation = readSwitches() == 4;
                        }
                    }
                }

                if (abortOperation == true ) {
                    connectedToPC = false;
                    D_LEDS_OFF();
                    break;
                }
            }
        }

    }
}

// Waits until detects block.
// true if block detected, false if cancelled/connected to PC.
bool waitForBlock()
{
    if (connectedToPC == false) {
        bool abortOperation = false;
        int blockInserted = 0;
        // Wait until a block is breaking the beam, or button 4 is pressed to abort.
        do {
            blockInserted = fpga->getBeamValue(Top);
            myLED4 = blockInserted;
            if (i2cport->read_bit(11)) {
                abortOperation = displayAbortDialog();
                // Cancel the Abort
                if (abortOperation == false) {
                    displayWaitingLine();
                    lcd->printf("for block");
                    i2cport->write_bit(1, 15);
                }
            }
        } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);

        if (abortOperation == true || connectedToPC == true)
            return false;
        else
            return true;
    } else if (connectedToPC == true) {
        bool abortOperation = false;
        int blockInserted = 0;
        // Wait until a block is breaking the beam, or button 4 is pressed to abort.
        do {
            blockInserted = fpga->getBeamValue(Top);
            myLED4 = blockInserted;
            if (i2cport->read_bit(11)) {
                abortOperation = displayAbortDialog();
                // Cancel the Abort
                if (abortOperation == false) {
                    displayWaitingLine();
                    lcd->printf("for block");
                    i2cport->write_bit(1, 15);
                }
            }
        } while (abortOperation == false && blockInserted != 1 && connectedToPC == true);

        if (abortOperation == true || connectedToPC == false)
            return false;
        else
            return true;
    }
}

void sortBlock()
{
    // Cannot Abort any longer. Block is inserted.
    // Detach rx interrupt until block processed.
    NVIC_DisableIRQ(UART1_IRQn);
    int colourValues[4];
    rgbSensor.getAllColors(colourValues);

    int canCheckForSize = fpga->checkForBlock();
    int blockSize = 0;

    while (canCheckForSize == 0) {
        canCheckForSize = fpga->checkForBlock();
    }

    blockSize = fpga->checkForSize();
    myLED3 = blockSize;
    bool haz = false;

    if (blockSize == HazBlock->size) {
        haz = checkColour(colourValues);
    } else {
        fpga->moveStoppingServo(Go);
        while(fpga->checkForSize()) {}
        fpga->moveStoppingServo(Stop);
    }

    if (connectedToPC)
        pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i,Haz:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);

    myLED3 = 0;
    // Re-Attach rx interrupt
    NVIC_EnableIRQ(UART1_IRQn);

    return;
}

/// Called every-time it receives an char from PC.
void Rx_interrupt()
{
    char interruptChar = pc.getc();
    // Uncomment to Echo to USB serial to watch data flow
    //    pc.putc(interruptChar);

    NVIC_DisableIRQ(UART1_IRQn);

    if (interruptChar == CommandTypeValue[Query]) {
        commander->decodeCommand(Query);
    } else if (interruptChar == CommandTypeValue[Set]) {
        commander->decodeCommand(Set);
    } else if (interruptChar== CommandTypeValue[Reply]) {
        commander->decodeCommand(Reply);
    }

    NVIC_EnableIRQ(UART1_IRQn);
}

void initInternal()
{
    i2cport = new MCP23017(p9, p10, 0x40);
    lcd = new WattBob_TextLCD(i2cport);
    myLED1 = 1;
    BACKLIGHT_ON(i2cport);
    lcd->cls();
    LCDFL();
    lcd->printf("Initilizing...");
    myLED2 = 1;
    return;
}

void initPort(int baudRate)
{
    myLED3 = 1;
    pc.baud(baudRate);
    pc.format(8, SerialBase::None, gStopBits);
    myLED4 = 1;
    wait (0.1);
    return;
}

bool checkColour(int colourValues[])
{
    bool isHazColour[4] = {false, false, false, false };

    for (int i = 0; i < 4; i++) {
        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
            isHazColour[i] = true;
        }
    }

    fpga->moveSortingServo(Haz);
    fpga->moveStoppingServo(Go);
    while(fpga->checkForBlock()) {}
    fpga->moveStoppingServo(Stop);
    wait(0.5);
    fpga->moveSortingServo(NonHaz);
    return true;

    bool isHazBlock = false;

    if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]) {
        fpga->moveSortingServo(Haz);
        fpga->moveStoppingServo(Go);
        while(fpga->checkForBlock()) {}
        fpga->moveStoppingServo(Stop);
        fpga->moveSortingServo(NonHaz);
        return true;
    } else {
        fpga->moveStoppingServo(Go);
        while(fpga->checkForBlock()) {}
        fpga->moveStoppingServo(Stop);
        return false;
    }
}

void printPCDetectedText()
{
    lcd->cls();
    LCDFL();
    lcd->printf("Detected PC.");
    LCDSL();
    lcd->printf("Connecting");
    initPort();
}

bool displayAbortDialog()
{
    while (i2cport->read_bit(11) == 1) {}
    i2cport->write_bit(1, 12);

    lcd->cls();
    LCDFL();
    lcd->printf("Abort?");
    LCDSL();
    lcd->printf("1:Yes, 2,3,4:No");
    int reply = 0;
    do {
        reply = readSwitches();
    } while(reply == 0);

    D_LEDS_OFF();
    if (reply == 1) {
        //		while (i2cport->read_bit(8)) {  }
        return true;
    } else {
        //		while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) {  }
        return false;
    }
}

void printServoInfoOnLCD()
{
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
    lcd->locate(1,0);
    lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
}

void runInServoTestMode()
{
    pc.printf("VALUES:Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i\n:VALUES", Stop, Go, Haz, NonHaz);
    printServoInfoOnLCD();
    int finished = false;

    i2cport->write_bit(1, 12);
    i2cport->write_bit(1, 13);
    i2cport->write_bit(1, 15);
    int button = 0;
    do {
        button = readSwitches();
        if (button == 1) {
            fpga->toggleStoppingServo();
            printServoInfoOnLCD();
            wait(0.5);
        } else if (button == 2) {
            fpga->toggleSortingServo();
            printServoInfoOnLCD();
            wait(0.5);
        }
        finished = button == 4;
        button = 0;
    } while (finished == false && runServoTest != false);

    D_LEDS_OFF();
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Done servo test");
    //TODO: Inform PC of end.
    wait(0.5);
    lcd->cls();
    return;
}

void displayWaitingLine()
{
    lcd->cls();
    lcd->locate(0,0);
    lcd->printf("Waiting...");
    lcd->locate(1,0);
}

void displayPCStatus()
{
    lcd->cls();
    lcd->locate(0,0);
    if (connectedToPC) {
        if (currentMode == Normal)
            lcd->printf("Normal mode.");
        else if (currentMode == Maintanence)
            lcd->printf("Maintanence.");
        else if (currentMode == None)
            lcd->printf("Connected to PC");
    } else
        lcd->printf("Waiting for PC..");

    lcd->locate(1,0);
    lcd->printf("4:Disconnect");
    D_LEDS_OFF();
    i2cport->write_bit(1,15);
}