File content as of revision 10:16ba52f8e025:

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

void detectPC();
void initInternal();
void initPort(int baudRate=kDefaultBaudRate);
void printPCDetectedText();
bool displayAbortDialog();
void Rx_interrupt();
void sortBlock(int colourValues[]);
void runInServoTestMode();

void displayWaitingLine(){
	lcd->cls();
	lcd->locate(0,0);
    lcd->printf("Waiting...");
    lcd->locate(1,0);
}

int main()
{
    initInternal();
    initPort();
    srand((unsigned)time(NULL));
    U_LEDS_OFF();
    lcd->cls();
    myLED1 = 1;
    
    rgbSensor.enablePowerAndRGBC();
    rgbSensor.setIntegrationTime(100);

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

            bool abortOperation = false;
            for(;;) {
                displayWaitingLine();

                i2cport->write_bit(1, 15);


                int blockInserted = 0;
                // Wait until a block is breaking the beam, or button 4 is pressed to abort.
                do {
                    blockInserted = fpga->checkForBlock();
                    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);

                // Break and return to main menu i.e. Start Op, or Connect to PC.
                if (abortOperation == true || connectedToPC == true) {
                    D_LEDS_OFF();
                    break;
                }
				
                // Cannot Abort any longer. Block is inserted.
                // Detach rx interrupt until block processed.
                NVIC_DisableIRQ(UART1_IRQn);

                int blockSize = 0;
                blockSize = fpga->checkForSize();
                myLED3 = blockSize;

                if (blockSize == 0) {
                    //                        detectColour()
                    int colourValues[4];
                    displayWaitingLine();
//                    rgbSensor.getAllColors(colourValues);
                    lcd->cls();
                    lcd->locate(0,0);
                    lcd->printf("Sorting");
                    sortBlock(colourValues);
                }else {
                	fpga->moveStoppingServo(Go);
                	while(fpga->checkForSize()){}
                	fpga->moveStoppingServo(Stop);
                }
				
				myLED3 = 0;
                // Re-Attach rx interrupt
                NVIC_EnableIRQ(UART1_IRQn);
            }
//			if (abortOperation == true || connectedToPC == true){ break; }
        }

        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 abortOperation = false;
                while (connectedToPC == false && abortOperation == false) {
                    abortOperation = readSwitches() == 4;
                }
                if (abortOperation == true) {
                    D_LEDS_OFF();
                    break;
                } else if (connectedToPC == true) {
                    lcd->cls();
                    LCDFL();
                    lcd->printf("Connected to PC");
                    LCDSL();
                    lcd->printf("4: Disconnect.");
                }

                while (abortOperation == false && connectedToPC == true) {
					if (runServoTest == true){
                    	runInServoTestMode();
                    	lcd->cls();
                    	lcd->locate(0,0);
                    	lcd->printf("Connected to PC");
                    	lcd->locate(1,0);
                    	lcd->printf("4:Disconnect");
                    	i2cport->write_bit(1,15);
                    }
                    abortOperation = readSwitches() == 4;
                }

                if (abortOperation == true ) {
                    connectedToPC = false;
                    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;
}

void sortBlock(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;
	
	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);
	}else {
		fpga->moveStoppingServo(Go);
		while(fpga->checkForBlock()) {}
		fpga->moveStoppingServo(Stop);
	}
}

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) {
        return true;
    } else {
        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("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", 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");
    wait(0.5);
    lcd->cls();
    return;
}