File content as of revision 6:98fe30430194:

#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];

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

extern Block _HazBlock;
Block *HazBlock = &_HazBlock;

bool connectedToPC = false;

void detectPC();
void initInternal();
void initPort(int baudRate=kBaudRate);
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 (;;) {
		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 autonomously.
			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 = fpga->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 blockSize = fpga->checkForSize();

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

		} else if (selection == 2) {
			// Wait for PC to send '!<pc>init;' command.
			lcd->cls();
			LCDFL();
			lcd->printf("Waiting for PC...");
			LCDSL();
			lcd->printf("4: Main Menu");

			int aboardOperation = false;
			while (connectedToPC == false && aboardOperation == false){
				aboardOperation = readSwitches() == 4;
			}
			if (aboardOperation == true) { break; }
			else if (connectedToPC == true){
				lcd->cls();
				LCDFL();
				lcd->printf("Connected to PC");
				LCDSL();
				lcd->printf("4: Disconnect.");
			}

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

	}
}

/// Called every-time it receives an char from PC.
void Rx_interrupt(){

}

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=kBaudRate)
{
	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()
{
	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;
	}
}