File content as of revision 4:4eebb4de22a7:

#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 rgb_sensor(p28, p27);
Serial      pc(USBTX, USBRX);
uint8_t     rxBuffer[5 + 1];

Command     *myCommand;
bool connectedToPC = false;

void detectPC();

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=gBaudRate)
{
    myLED3 = 1;
    pc.baud(baudRate);
    pc.format(8, SerialBase::None, gStopBits);
    myLED4 = 1;
    wait (0.1);
    return;
}

int readSwitches()
{
    do {
        if(i2cport->read_bit(8)) {
            return 1;
        } else if (i2cport->read_bit(9)) {
            return 2;
        } else if (i2cport->read_bit(10)) {
            return 3;
        } else if (i2cport->read_bit(11)) {
            return 4;
        } else {
            return 0;
        }
    } while (1);
}

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

bool displayAboardDialog()
{
    lcd->cls();
    LCDFL();
    lcd->printf("Aboard operation?");
    LCDSL();
    lcd->printf("1: YES, 2,3,4: NO");
    int reply = readSwitches();
    if (reply == 1) {
        return true;
    } else {
        return false;
    }
}

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

    // 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

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

        int selection = 0;
        do {
            selection = readSwitches();
        } while (selection == 1 || selection == 2);

        if (selection == 1) {
            // User selected op 1: Start sorting autonomaously.
            i2cport->write_bit(0, 13);
            lcd->cls();
            LCDFL();
            lcd->printf("Starting sorting operation");
            wait(1);

            for(;;) {
                lcd->cls();
                lcd->printf("Waiting for block");
                i2cport->write_bit(1, 15);
                bool aboardOperation = false;

                int blockInserted = 0;
                // Wait until a block is breaking the beam, or button 4 is pressed to aboard.
                do {
                    blockInserted = checkForBlock();
                    if (i2cport->read_bit(11)) {
                        aboardOperation = displayAboardDialog();
                        // Cancel the aboard
                        if (aboardOperation == false) {
                            lcd->cls();
                            LCDFL();
                            lcd->printf("Waiting for block");
                        }
                    }
                } while (aboardOperation == false && blockInserted != 1);

                // Break and return to main menu i.e. Start Op, or Connect to PC.
                if (aboardOperation == true) {
                    break;
                }
                // Cannot aboard any longer. Block is inserted.
                int largeBlock = blockIsLarge();

                if (largeBlock == hazBlock.blockSizeEnum) {
//                        detectColour()
                    lcd->cls();
                    lcd->printf("Detecting Colour");
                }
            }

        }

        else if (selection == 2) {
            // Wait for PC to send '$' char to make sure it's there.
            lcd->cls();
            LCDFL();
            lcd->printf("Waiting for PC...");
            LCDSL();
            lcd->printf("4: Main Menu");

        }

    }


    FOREVER {
        pc.printf("In FOREVER\n");
        Type type = INVALID;
        pc.printf("Type set to: %i\r\n", type);
        do {
            pc.printf("In DO-WHILE loop\r\n");
            type = listenForCommand();
            if (i2cport->read_bit(8)) {
                pc.printf("Listening\r\n");
            }
        } while (type == -1);

        pc.printf("Command Inbound\r\n");

        myCommand.typeEnum = type;
        pc.printf("Type: %i\r\n-------------\r\n", myCommand.typeEnum);
        recieveCommand(&myCommand, type);

        pc.printf("inputstring:%s\r\n%s\r\n<%s>\r\n%s;\r\n", myCommand.inputString, myCommand.commandType, myCommand.commandObject, myCommand.commandString);
//            decodeCommand(&myCommand);


    }
}