![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
3rd year group project. Electronic and Electrical Engineering. Heriot-Watt University. This is the code for the mbed for the Automatic Little Object Organiser (ALOO).
Dependencies: MCP23017 TCS3472_I2C WattBob_TextLCD mbed
Diff: main.cpp
- Revision:
- 20:4e0f0944f28f
- Parent:
- 19:61b21ac4896e
- Child:
- 21:60c4fe94d79c
--- a/main.cpp Tue Nov 24 01:38:46 2015 +0000 +++ b/main.cpp Tue Nov 24 20:53:51 2015 +0000 @@ -24,7 +24,6 @@ MCP23017 *i2cport; WattBob_TextLCD *lcd; - TCS3472_I2C rgbSensor(p28, p27); Serial pc(USBTX, USBRX); uint8_t rxBuffer[kSmallBufferSize + 1]; @@ -35,6 +34,7 @@ FPGA _fpga = FPGA(); extern FPGA *fpga = &_fpga; +Block defualtHazBlock = Block(); extern Block _HazBlock; Block *HazBlock = &_HazBlock; @@ -43,7 +43,7 @@ void initInternal(); void initPort(int baudRate=kDefaultBaudRate); void printPCDetectedText(); -bool displayAbortDialog(); +//bool displayAbortDialog(); void Rx_interrupt(); bool checkColour(int colourValues[]); void runInServoTestMode(); @@ -59,10 +59,9 @@ srand((unsigned)time(NULL)); U_LEDS_OFF(); lcd->cls(); - myLED1 = 1; rgbSensor.enablePowerAndRGBC(); - rgbSensor.setIntegrationTime(2.4); + rgbSensor.setIntegrationTime(3); // 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 @@ -141,24 +140,30 @@ lcd->locate(1,0); lcd->printf("1: Start"); int button = 0; - while (currentState == Pause) { + i2cport->write_bit(1, 12); + i2cport->write_bit(1, 15); + while (currentState == Pause && currentMode == Normal) { button = readSwitches(); if (button == 1) { pc.printf("state:start\n"); currentState = Start; } else if (button == 4) { - goto setModeNone; + currentMode = None; +// goto setModeNone; + break; } } } if (currentState == Start) { - while (currentState == Start) { + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Sorting mode..."); + while (currentState == Start && currentMode == Normal) { if (waitForBlock() == false) { - currentState = Pause; if (connectedToPC == true) { // TODO: Tell PC to update UI if aborted from MBED. pc.printf("state:pause\n"); - + currentState = Pause; continue; } else if (connectedToPC == false) { currentMode = None; @@ -172,9 +177,18 @@ } } else if (currentMode == None) { setModeNone: + D_LEDS_OFF(); + i2cport->write_bit(1,15); displayPCStatus(); - while (currentMode == None && abortOperation == false) { - abortOperation = readSwitches() == 4; + 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); + } + } } } } @@ -194,6 +208,8 @@ // 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; @@ -232,9 +248,9 @@ i2cport->write_bit(1, 15); } } - } while (abortOperation == false && blockInserted != 1 && connectedToPC == true); + } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal); - if (abortOperation == true || connectedToPC == false) + if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence) return false; else return true; @@ -243,6 +259,8 @@ void sortBlock() { + myLED1 = 0; + myLED2 = 1; // Cannot Abort any longer. Block is inserted. // Detach rx interrupt until block processed. NVIC_DisableIRQ(UART1_IRQn); @@ -257,7 +275,6 @@ } blockSize = fpga->checkForSize(); - myLED3 = blockSize; bool haz = false; if (blockSize == HazBlock->size) { @@ -268,13 +285,14 @@ 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); - + myLED3 = 0; + myLED4 = 1; return; } @@ -300,13 +318,14 @@ void initInternal() { + myLED1 = 1; i2cport = new MCP23017(p9, p10, 0x40); lcd = new WattBob_TextLCD(i2cport); - myLED1 = 1; BACKLIGHT_ON(i2cport); lcd->cls(); - LCDFL(); + lcd->locate(0,0); lcd->printf("Initilizing..."); + DefaultHazBlock(); myLED2 = 1; return; } @@ -323,37 +342,53 @@ bool checkColour(int colourValues[]) { - bool isHazColour[4] = {false, false, false, false }; + myLED2 = 0; + myLED3 = 1; + bool isHazColour[3] = {false, false, false }; + + float adjustedValues[3]; + float percentageError[3]; + - for (int i = 0; i < 4; i++) { - if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { + for (int i = 0; i < 3; i++) { + adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; + percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i]; + pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]); + + if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0) { + pc.printf("DEBUG:%i.\n", i); isHazColour[i] = true; + } else if (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2) { + isHazColour[i] = true; + pc.printf("DEBUG:%i.\n", i); } } - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - wait(0.5); - fpga->moveSortingServo(NonHaz); - return 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] && isHazColour[3]) { + if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { fpga->moveSortingServo(Haz); fpga->moveStoppingServo(Go); while(fpga->checkForBlock()) {} + wait(kServoWait); fpga->moveStoppingServo(Stop); fpga->moveSortingServo(NonHaz); - return true; + isHazBlock = true; } else { fpga->moveStoppingServo(Go); while(fpga->checkForBlock()) {} fpga->moveStoppingServo(Stop); - return false; + isHazBlock = false; } + + pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], isHazBlock); + return isHazBlock; } void printPCDetectedText() @@ -415,11 +450,11 @@ if (button == 1) { fpga->toggleStoppingServo(); printServoInfoOnLCD(); - wait(0.5); + wait(kServoWait); } else if (button == 2) { fpga->toggleSortingServo(); printServoInfoOnLCD(); - wait(0.5); + wait(kServoWait); } finished = button == 4; button = 0;