![](/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
main.cpp
- Committer:
- dreamselec
- Date:
- 2015-12-03
- Revision:
- 31:16e056b9f9e0
- Parent:
- 30:c0bc92d009fe
- Child:
- 32:9a4046224b11
File content as of revision 31:16e056b9f9e0:
#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]; int blockCount = 0; bool lastBlockHaz = false; bool recievingResponse = false; bool isBetweenHazValues[3] = { false, false, false }; 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(); void newHazBlockMode(); int main() { initInternal(); initPort(); for (;;) { if (connectedToPC == false) { pc.printf("DEBUG: PC did not responded.\n"); lcd->cls(); i2cport->write_bit(1, 12); lcd->locate(0,0); lcd->printf("1: Start sorting."); lcd->locate(1,0); 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); // lcd->cls(); 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(;;) { if (blockCount > 0) { lcd->cls(); displayWaitingLine(); lcd->locate(1,0); if (lastBlockHaz == true) lcd->printf("Hazardous"); else if (lastBlockHaz == false) lcd->printf("Non-Hazardous"); } else { displayWaitingLine(); } 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) { wait(0.1); 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(); else if (setNewHazBlock == true) newHazBlockMode(); 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 && currentMode == Normal) { // TODO: Tell PC to update UI if aborted from MBED. pc.printf(":<mbed>sort=pause;"); currentState = Pause; continue; } else if (connectedToPC == true && currentMode == None) { goto setModeNone; } else if (connectedToPC == false) { currentMode = None; currentState = Pause; 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(); } } } 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. pc.printf("INFO: Waiting for block.\n"); do { blockInserted = fpga->getBeamValue(Top); myLED4 = blockInserted; if (readSwitches() == 4) { 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 != Normal) return false; else return true; } return false; } void sortBlock() { pc.printf("BLOCK: Sorting block"); 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}; int numberOfReadings = 3; for (int i = 0; i < 4; 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] / numberOfReadings; } lastBlockHaz = false; lastBlockHaz = checkColour(averageColourValues); // lastBlockHaz = checkColour(colourValues); if (!lastBlockHaz) { fpga->moveSortingServo(NonHaz); } myLED3 = 1; int blockSize; while (fpga->checkForBlock() == 0) { } blockSize = fpga->checkForSize(); if (blockSize == HazBlock->size && lastBlockHaz) { // fpga->moveSortingServo(Haz); // fpga->moveStoppingServo(Go); // blockSize = HazBlock->size; while(fpga->getBeamValue(Bottom) == 1) {} wait(kServoWait); fpga->moveStoppingServo(Stop); } else { lastBlockHaz = false; } fpga->moveSortingServo(NonHaz); while(fpga->checkForSize()) {} if (connectedToPC) { for (int i = 0; i < 3; i++) { pc.printf("DEBUG:Percentage Error: %.5f. Passed?: %i\n", percentageError[i], isBetweenHazValues[i]); // if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinRedError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxRedError[i] * errorMultiplier)) // 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], lastBlockHaz, 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], lastBlockHaz); } // Re-Attach rx interrupt NVIC_EnableIRQ(UART1_IRQn); blockCount++; myLED3 = 0; myLED4 = 1; return; } /// Called every-time it receives an char from PC. void Rx_interrupt() { recievingResponse = true; 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); recievingResponse = false; } 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(); rgbSensor.enablePowerAndRGBC(); rgbSensor.setIntegrationTime(gIntegrationTime); srand((unsigned)time(NULL)); myLED2 = 1; fpga->moveStoppingServo(Stop); fpga->moveSortingServo(NonHaz); return; } void initPort(int baudRate) { myLED3 = 1; pc.baud(baudRate); pc.format(8, SerialBase::None, gStopBits); pc.attach(&Rx_interrupt, Serial::RxIrq); myLED4 = 1; pc.printf(":<pc>connect;"); // wait (0.3); // while(recievingResponse == true) {} turnOffTopLEDs(); return; } bool checkColour(int colourValues[]) { for (int i = 0; i < 3; i++) { isBetweenHazValues[i] = 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)) { isBetweenHazValues[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]; // Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set. 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++) { adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i]; if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) { isBetweenHazValues[i] = true; } } bool isHazBlock = false; if (isBetweenHazValues[0] && isBetweenHazValues[1] && isBetweenHazValues[2]) { isHazBlock = true; } else { isHazBlock = false; } myLED2 = 0; return isHazBlock; } void newHazBlockMode() { trySetHazBlockAgain: pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour); fpga->moveSortingServo(Haz); // pc.printf("INFO:Setting new haz block.\n"); int lowerBeam = 0; int higherBeam = 0; int colourValues[6][4]; for (int i = 0; i < 6; i++) { memset(colourValues[i], 0, sizeof(colourValues[6])); } int readingCount = 0; lcd->cls(); lcd->locate(0,0); lcd->printf("New haz block"); do { higherBeam = fpga->getBeamValue(Top); if (readSwitches() == 4) { if (displayAbortDialog()) { //TODO: tell pc pc.printf(":<mbed>haz-block=pause;"); pc.printf("INFO: Operation aborted form MBED.\n"); fpga->moveSortingServo(NonHaz); displayPCStatus(); return; } else { lcd->cls(); lcd->locate(0,0); lcd->printf("New haz block"); } } } while (higherBeam != 1 && setNewHazBlock == true && connectedToPC == true && currentMode == Maintanence); if (setNewHazBlock == false) { displayPCStatus(); return; } do { rgbSensor.getAllColors(colourValues[readingCount]); readingCount++; } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0); lowerBeam = fpga->getBeamValue(Bottom); higherBeam = fpga->getBeamValue(Top); pc.printf("DEBUG: Number of readings: %i\n", readingCount); if (lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Top); while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); } higherBeam = fpga->getBeamValue(Bottom); } int blockSize = higherBeam; int totalComponents[4]; memset(totalComponents, 0, sizeof(totalComponents)); for (int k = 0; k < readingCount; k++) { for (int i = 0; i < 4; i++) { totalComponents[i] += colourValues[k][i]; } } pc.printf("VALUE: Total: %i, %i, %i, %i \n:VALUE", totalComponents[0], totalComponents[1], totalComponents[2], totalComponents[3]); float averageComponents[4]; memset(averageComponents, 0, sizeof(averageComponents)); for (int i = 0; i < 4; i++) { averageComponents[i] = ((float)totalComponents[i] / (float)readingCount); } pc.printf("VALUE: Average: %i, %i, %i, %i \n:VALUE", averageComponents[0], averageComponents[1], averageComponents[2], averageComponents[3]); float adjustedValues[3]; memset(adjustedValues, 0, sizeof(adjustedValues)); for (int i = 0; i < 3; i++) { adjustedValues[i] = averageComponents[i] / averageComponents[3]; } pc.printf("VALUE: Adjusted: %.3f, %.3f, %.3f \n:VALUE", adjustedValues[0], adjustedValues[1], adjustedValues[2]); Block::BlockColour detectedColour = Block::Wrong; bool matchesColour[3] = {false, false, false}; float percentageError[3]; memset(percentageError, 0, sizeof(percentageError)); for (int k = 0; k < 7; k++) { pc.printf("DEBUG: Colour: %i\n", k); for (int i = 0; i < 3; i++) { percentageError[i] = (adjustedValues[i] - kAverageValues[k][i]) / kAverageValues[k][i]; pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[i]); if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[k][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[k][i] * errorMultiplier)) { pc.printf("DEBUG: Pass\n"); matchesColour[i] = true; } } if (matchesColour[0] && matchesColour[1] && matchesColour[2]) { detectedColour = static_cast<Block::BlockColour>(k); pc.printf("DEBUG: Found match as: %i", detectedColour); break; } else { matchesColour[0] = false; matchesColour[1] = false; matchesColour[2] = false; } } if (detectedColour == Block::Wrong) { pc.printf("ERROR: Could not detect colour.\n"); lcd->cls(); lcd->printf("1: Try again"); lcd->locate(1,0); lcd->printf("2: Revert to last"); int button = 0; do { button = readSwitches(); if (button == 1 || setNewHazBlock == true) { goto trySetHazBlockAgain; } } while (button != 2); } // Point and literal might not sync... _HazBlock.size = static_cast<Block::Size>(blockSize); _HazBlock.colour = detectedColour; pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour); pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", _HazBlock.size, kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][1], kMinError[_HazBlock.colour][2], kMaxError[_HazBlock.colour][0], kMaxError[_HazBlock.colour][1], kMaxError[_HazBlock.colour][2]); pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[_HazBlock.colour][0], kAverageValues[_HazBlock.colour][1], kAverageValues[_HazBlock.colour][2], kAverageValues[_HazBlock.colour][3]); fpga->moveSortingServo(NonHaz); } 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"); fpga->moveSortingServo(NonHaz); 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(); lcd->locate(0,0); lcd->printf("Colour sensor"); lcd->locate(1,0); lcd->printf("Test mode"); } 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) { lcd->cls(); lcd->locate(0,0); lcd->printf("Drop block"); turnOffTopLEDs(); while(fpga->getBeamValue(Top) == 0) { myLED1 = fpga->getBeamValue(Top); myLED2 = fpga->getBeamValue(Bottom); } myLED3 = 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]); valueCount++; } while (fpga->getBeamValue(Top) == 1 && valueCount < 3); turnOffTopLEDs(); myLED4 = 1; 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; myLED4 = 0; 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 == true) { 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); }