File content as of revision 26:bbcc25418ffa:

#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;
float 		percentageError[3];
float 		adjustedValues[3];

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

Block defualtHazBlock = Block();
extern Block _HazBlock;
Block *HazBlock = &_HazBlock;

extern PCModes currentMode;

void initInternal();
void initPort(int baudRate=kDefaultBaudRate);
void printPCDetectedText();
void Rx_interrupt();
bool checkColour(int colourValues[]);
void runInServoTestMode();
void displayWaitingLine();
void displayPCStatus();
bool waitForBlock();
void sortBlock();
void runInBreakBeamTestMode();
void turnOffTopLEDs();
void turnOffBottomLEDs();
void runInColourSensorTestMode();

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

	rgbSensor.enablePowerAndRGBC();
	rgbSensor.setIntegrationTime(gIntegrationTime);

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

	DefaultHazBlock();
			fpga->moveStoppingServo(Stop);
		fpga->moveSortingServo(NonHaz);
				pc.printf(":<pc>connect;");
	
	for (;;) {
		if (connectedToPC == false){
		lcd->cls();
		i2cport->write_bit(1, 12);
		lcd->printf("1: Start sorting.");
		LCDSL();
		i2cport->write_bit(1,13);
		lcd->printf("2: Connect to PC");
		}
		
//		wait(0.03);

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

				// 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;
				if (connectedToPC == false)
					pc.printf(":<pc>connect;");
				while (connectedToPC == false && abortOperation == false) {
					abortOperation = readSwitches() == 4;
				}

				if (abortOperation == true) {
					D_LEDS_OFF();
					break;
				}

				displayPCStatus();

				while (abortOperation == false && connectedToPC == true) {
					if (currentMode == Maintanence) {
						displayPCStatus();
						while (currentMode == Maintanence){
						if (runServoTest == true)
							runInServoTestMode();
						else if (runBreakBeamTest == true)
							runInBreakBeamTestMode();
						else if (runColourSensorTest == true)
							runInColourSensorTestMode();
						if (i2cport->read_bit(11) == 1){
							if (displayAbortDialog() == true){
								currentMode = None;
								pc.printf(":<mbed>mode=none;");
							}else {
								displayPCStatus();
							}
						}
						}
					} 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;
								i2cport->write_bit(1, 12);
								i2cport->write_bit(1, 15);
								while (currentState == Pause && currentMode == Normal) {
									button = readSwitches();
									if (button == 1) {
										pc.printf(":<mbed>sort=start;");
										currentState = Start;
									} else if (button == 4) {
										currentMode = None;
										pc.printf(":<mbed>mode=none;");
										//                                        goto setModeNone;
										break;
									}
								}
							}
							if (currentState == Start) {
								lcd->cls();
								lcd->locate(0,0);
								lcd->printf("Sorting mode...");
								while (currentState == Start && currentMode == Normal) {
									if (waitForBlock() == false) {
										if (connectedToPC == true && currentState != Pause) {
											// TODO: Tell PC to update UI if aborted from MBED.
											pc.printf(":<mbed>sort=pause;");
											currentState = Pause;
											continue;
										} else if (connectedToPC == false) {
											currentMode = None;
											abortOperation = true;
										}
									} else {
										sortBlock();
									}
								}
							}
						}
					} else if (currentMode == None) {
						setModeNone:
						D_LEDS_OFF();
						i2cport->write_bit(1,15);
						displayPCStatus();
						while (currentMode == None && abortOperation == false && connectedToPC == true) {
							if (i2cport->read_bit(11)) {
								abortOperation = displayAbortDialog();
								// Cancel the Abort
								if (abortOperation == false) {
									displayPCStatus();
									i2cport->write_bit(1, 15);
								}
							}
						}
					}
				}

				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()
{
	myLED4 = 0;
	myLED1 = 1;
	if (connectedToPC == false) {
		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;
	} else if (connectedToPC == true) {
		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 == true && currentState == Start && currentMode == Normal);

		if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
			return false;
		else
			return true;
	}
	return false;
}

void sortBlock()
{
	myLED1 = 0;
	myLED2 = 1;
	// Cannot Abort any longer. Block is inserted.
	// Detach rx interrupt until block processed.
	NVIC_DisableIRQ(UART1_IRQn);
	fpga->moveSortingServo(Haz);
	fpga->moveStoppingServo(Go);

	int colourValues[4];
	int averageColourValues[4] = {0, 0, 0, 0};
	for (int i = 0; i < 3; i++){
		rgbSensor.getAllColors(colourValues);
		for (int j = 0; j < 4; j++){
			averageColourValues[j] += colourValues[j];
		}
	}
	for (int i = 0; i < 4; i++){
		averageColourValues[i] = averageColourValues[i] / 3;
	}

	bool haz = false;
	haz = checkColour(colourValues);

	if (!haz) {
		fpga->moveSortingServo(NonHaz);
	}
	int blockSize;
	while (fpga->checkForBlock() == 0) {  }
	blockSize = fpga->checkForSize();
	if (blockSize == HazBlock->size && haz) {
		//        fpga->moveSortingServo(Haz);
		//        fpga->moveStoppingServo(Go);
		//        blockSize = HazBlock->size;
		while(fpga->getBeamValue(Bottom) == 1) {}
		wait(kServoWait);
		fpga->moveStoppingServo(Stop);
	} else {
		haz = false;
	}
	fpga->moveSortingServo(NonHaz);
	while(fpga->checkForSize()) {}

	if (connectedToPC) {
		for (int i = 0; i < 3; i++) {
			pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
			if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
				pc.printf("DEBUG:%i Pass.\n", i);
		}
		pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
		//        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
	}

	// Re-Attach rx interrupt
	NVIC_EnableIRQ(UART1_IRQn);
	myLED3 = 0;
	myLED4 = 1;
	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()
{
	myLED1 = 1;
	i2cport = new MCP23017(p9, p10, 0x40);
	lcd = new WattBob_TextLCD(i2cport);
	BACKLIGHT_ON(i2cport);
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Initilizing...");
	DefaultHazBlock();
	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[])
{
	myLED2 = 0;
	myLED3 = 1;
	bool isHazColour[3] = {false, false, false };
	memset(adjustedValues, 0, sizeof(adjustedValues));
	memset(percentageError, 0, sizeof(percentageError));

/* Harcoded working
	for (int i = 0; i < 3; i++) {
		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];

		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
			isHazColour[i] = true;
		}
	}
*/
	for (int i = 0; i < 3; i++) {
		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];

		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
			isHazColour[i] = true;
		}
	}
	
	//    for (int i = 0; i < 3; i++) {
		//        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
	//            isHazColour[i] = true;
	//        }
	//    }

	bool isHazBlock = false;

	if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
		isHazBlock = true;
	} else {
		isHazBlock = false;
	}

	return isHazBlock;
}

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);
	if (fpga->stoppingServoPosition == Stop)
		lcd->printf("1:Top: Go");
	else if (fpga->stoppingServoPosition == Go)
		lcd->printf("1:Top: Stop");

	lcd->locate(1,0);
	if (fpga->sortingServoPosition == NonHaz)
		lcd->printf("2:Bottom: Haz");
	else if (fpga->sortingServoPosition == Haz)
		lcd->printf("2:Bottom: NonHaz");
}

void printServoInfoOnPC() {
	if (fpga->stoppingServoPosition == Stop)
		pc.printf(":<servos>1=Stop;");
	else if (fpga->stoppingServoPosition == Go)
		pc.printf(":<servos>1=Go;");

	if (fpga->sortingServoPosition == NonHaz)
		pc.printf(":<servos>2=NonHaz;");
	else if (fpga->sortingServoPosition == Haz)
		pc.printf(":<servos>2=Haz;");
}

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);
	printServoInfoOnPC();
	printServoInfoOnLCD();

	i2cport->write_bit(1, 12);
	i2cport->write_bit(1, 13);
	i2cport->write_bit(1, 15);
	int button = 0;
	bool finished = false;
	do {
		button = readSwitches();
		
		// gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
		if (gToggleServoNumber == 1){
			button = 1;
			gToggleServoNumber = 0;
		}else if (gToggleServoNumber == 2){
			button = 2;
			gToggleServoNumber = 0;
		}
		
		if (button == 1) {
			fpga->toggleStoppingServo();
			printServoInfoOnLCD();
			printServoInfoOnPC();
			wait(kServoWait);
		} else if (button == 2) {
			fpga->toggleSortingServo();
			printServoInfoOnLCD();
			printServoInfoOnPC();
			wait(kServoWait);
		}
			
		finished = button == 4;
		button = 0;
	} while (finished == false && runServoTest == true);

	D_LEDS_OFF();
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Done servo test");
	if (runServoTest == true){
		pc.printf(":<servos>test=pause;");
		runServoTest = false;
	}
	wait(0.5);
	lcd->cls();
	return;
}

void printBeamInfoOnLCD(){
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Top:L1 Btm: L2");
	lcd->locate(1,0);
	lcd->printf("On:High, Off:Low");
}

void printBeamInfoOnPC(int topBeam, int bottomBeam){
	pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
}

void runInBreakBeamTestMode(){
	turnOffTopLEDs();
	i2cport->write_bit(1, 15);

	int topBeamValue = fpga->getBeamValue(1);
	int bottomBeamValue = fpga->getBeamValue(2);
	printBeamInfoOnPC(topBeamValue, bottomBeamValue);
	printBeamInfoOnLCD();

	int button = 0;
	bool finished = false;
	do{
		button = readSwitches();
		int currentTopBeamValue = fpga->getBeamValue(1);
		int currentBottomBeamValue = fpga->getBeamValue(2);
		
		// For debugging, hold down both 1 and 3 or 2 and 3
		if (i2cport->read_bit(10) == 1){
			currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
			currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
		}
		myLED1 = currentTopBeamValue;
		myLED2 = currentBottomBeamValue;

		if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ){
			topBeamValue = currentTopBeamValue;
			bottomBeamValue = currentBottomBeamValue;
			printBeamInfoOnPC(topBeamValue, bottomBeamValue);
		}
		finished = button == 4;
	}while (runBreakBeamTest == true && finished == false);

	turnOffBottomLEDs();
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Finished test");
	if (runBreakBeamTest == true){
		pc.printf(":<break_beam>test=pause;");
		runBreakBeamTest = false;
	}
	wait(0.5);
	return;
}

void printColourSensorInfoOnLCD(int colourValues[]){
	float weightedValues[4];
	
	for (int i = 0; i < 3; i++){
		weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
	}
	//TODO: Print values on LCD
	lcd->cls();
}

void printColourSensorInfoOnPC(int colourValues[]){
	pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
}

void runInColourSensorTestMode() {
	turnOffTopLEDs();
	i2cport->write_bit(1, 15);
	i2cport->write_bit(1, 12);
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Colour Sensor");
	lcd->locate(1,0);
	lcd->printf("Test Mode");
	pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
	
	int button = 0;
	bool finished = false;
	
	do {
		button = readSwitches();
		
		if (getColourSensorValue == true){
			int colourValues[4];
			rgbSensor.getAllColors(colourValues);
			printColourSensorInfoOnPC(colourValues);
			printColourSensorInfoOnLCD(colourValues);
			getColourSensorValue = false;
		} else if (getBlockColourValue == true){
			int topBeam = fpga->getBeamValue(1);
			
			while(topBeam == 0){
				topBeam = fpga->getBeamValue(1);
			}
			int colourValues[3][4];
			for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); }
			int valueCount = 0;
			do {
				rgbSensor.getAllColors(colourValues[valueCount]);
			} while (topBeam == 1 && valueCount < 3);
			int averageValues[4] = {0, 0, 0, 0};
			for (int i = 0; i < 3; i++) {
				averageValues[0] += colourValues[i][0];
				averageValues[1] += colourValues[i][1];
				averageValues[2] += colourValues[i][2];
				averageValues[3] += colourValues[i][3];
			}
			averageValues[0] = averageValues[0] / valueCount;
			averageValues[1] = averageValues[1] / valueCount;
			averageValues[2] = averageValues[2] / valueCount;
			averageValues[3] = averageValues[3] / valueCount;
			
			printColourSensorInfoOnPC(averageValues);
			printColourSensorInfoOnLCD(averageValues);
			
			getBlockColourValue = false;
		} else if (button == 1){
			getColourSensorValue = true;
			button = 0;
		}
		
		finished = button == 4;
//		button = 0;
	} while (finished == false && runColourSensorTest == true);
	
	turnOffBottomLEDs();
	lcd->cls();
	lcd->locate(0,0);
	lcd->printf("Finished test");
	if (runColourSensorTest == true){
		pc.printf(":<colour_sensor>test=pause;");
		runColourSensorTest = false;
	}
	wait(0.5);
	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);
}

void turnOffTopLEDs(){
	myLED1 = 0;
	myLED2 = 0;
	myLED3 = 0;
	myLED4 = 0;
}

void turnOffBottomLEDs(){
	i2cport->write_bit(0, 12);
	i2cport->write_bit(0, 13);
	i2cport->write_bit(0, 14);
	i2cport->write_bit(0, 15);
}