File content as of revision 9:dc8f155b71c8:

#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[kSmallBufferSize + 1];
int 		rxIndex = 0;

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

extern Block _HazBlock;
Block *HazBlock = &_HazBlock;

void detectPC();
void initInternal();
void initPort(int baudRate=kDefaultBaudRate);
int readSwitches();
void printPCDetectedText();
bool displayAboardDialog();
void Rx_interrupt();


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
	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");

		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(;;) {
				lcd->cls();
				lcd->printf("Waiting...");
				
				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 = fpga->checkForBlock();
					if (i2cport->read_bit(11)) {
						aboardOperation = displayAboardDialog();
						// Cancel the aboard
						if (aboardOperation == false) {
							lcd->cls();
							LCDFL();
							lcd->printf("Waiting...");
							i2cport->write_bit(1, 15);
						}
					}
				} while (aboardOperation == false && blockInserted != 1);

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

				// Cannot aboard any longer. Block is inserted.
				// Detach rx interrupt until block processed.
				NVIC_DisableIRQ(UART1_IRQn);

				int blockSize = fpga->checkForSize();

				if (blockSize == HazBlock->size) {
					//                        detectColour()
					lcd->cls();
					lcd->printf("Detecting Colour");
				}

				// Re-Attach rx interrupt
				NVIC_EnableIRQ(UART1_IRQn);
			}

		} else if (selection == 2 || connectedToPC == true) {
			for (;;){
                if (!connectedToPC) {
                    i2cport->write_bit(1, 13);
                    // Wait for PC to send '!<pc>connect;' command.
                    lcd->cls();
                    LCDFL();
                    lcd->printf("Waiting for PC...");
                    wait(0.5);
                    D_LEDS_OFF();
                    LCDSL();
                    lcd->printf("4: Main Menu");
                }
				i2cport->write_bit(1, 15);
				int aboardOperation = false;
				while (connectedToPC == false && aboardOperation == false){
					aboardOperation = readSwitches() == 4;
				}
				if (aboardOperation == true) { D_LEDS_OFF(); break; }
				else if (connectedToPC == true){
					lcd->cls();
					LCDFL();
					lcd->printf("Connected to PC");
					LCDSL();
					lcd->printf("4: Disconnect.");
				}

				while (aboardOperation == false && connectedToPC == true){
					aboardOperation = readSwitches() == 4;
				}
	
				if (aboardOperation == true ) { D_LEDS_OFF(); break; }
			}
		}

	}
}

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

int readSwitches()
{

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

}

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

bool displayAboardDialog()
{
	while (i2cport->read_bit(11) == 1){}
	i2cport->write_bit(1, 12);
	
	lcd->cls();
	LCDFL();
	lcd->printf("Aboard?");
	LCDSL();
	lcd->printf("1:YES, 2,3,4:NO");
	int reply = 0;
	do {
		reply = readSwitches();
	} while(reply == 0);

	D_LEDS_OFF();
	if (reply == 1) {
		return true;
	} else {
		return false;
	}
}