File content as of revision 18:44a1c1a30166:

#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);
				waitForBlock(&abortOpration);

				// 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){
										// TODO: Tell PC to update UI if aborted from MBED.
										pc.printf("state:pause\n");
										currentState = Pause;
										continue;
									}else {
										sortBlock();
									}
								}
							}
						}
					}else if (currentMode == None){
						setModeNone:
						displayPCStatus();
						while (currentMode == None) {}
					}
//					if (readSwitches() == 4) {
//						abortOperation = displayAbortDialog();
//						if (abortOperation == false)
//							displayPCStatus();
//					}
				}

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

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