File content as of revision 17:af373246bf80:

#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();
void displayPCStatus();

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();
                lcd->printf("for block.");

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

                // 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 canCheckForSize = fpga->checkForBlock();
                int blockSize = 0;

                while (canCheckForSize == 0) {
                    canCheckForSize = fpga->checkForBlock();
                }

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

                if (blockSize == Block::Small) {
                    //                        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 (;;) {
                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 (runServoTest == true)
                        runInServoTestMode();

                    if (readSwitches() == 4) {
                        abortOperation = displayAbortDialog();
                        if (abortOperation == false)
                            displayPCStatus();

                    }
                }

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