File content as of revision 3:843b830ee8bd:

#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 PRINT_LCD(TEXT) lcd->printf(TEXT);
#define CLEAR_LCD() lcd->cls();
#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);
    CLEAR_LCD();
    LCDFL();
    PRINT_LCD("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(){
    CLEAR_LCD();
    LCDFL();
    lcd->printf("Detected PC.");
    LCDSL();
    lcd->printf("Connecting");
    initPort();
}

bool displayAboardDialog(){
    CLEAR_LCD();
    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();
        CLEAR_LCD();
        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);
                CLEAR_LCD();
                LCDFL();
                lcd->printf("Starting sorting operation");
                wait(1);

                for(;;) {
                    CLEAR_LCD();
                    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 = blockIsInserted();
                        if (i2cport->read_bit(11)) {
                            aboardOperation = displayAboardDialog();
                            // Cancel the aboard 
                            if (aboardOperation == false) {
                                CLEAR_LCD();
                                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()
                        CLEAR_LCD();
                        lcd->printf("Detecting Colour");
                    }
                   } 
                    
                }

                else if (selection == 2) {
                    // Wait for PC to send '$' char to make sure it's there.
                    CLEAR_LCD();
                    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);
            
            
        }
}