![](/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:
- 30:c0bc92d009fe
- Parent:
- 29:9c0339e3c593
- Child:
- 31:16e056b9f9e0
--- a/main.cpp Tue Dec 01 22:56:08 2015 +0000 +++ b/main.cpp Tue Dec 01 23:50:29 2015 +0000 @@ -33,7 +33,7 @@ int blockCount = 0; bool lastBlockHaz = false; bool recievingResponse = false; -bool isBetweenHazValues[3] = false; +bool isBetweenHazValues[3] = { false, false, false }; Commander _commander = Commander(); Commander *commander = &_commander; @@ -60,6 +60,7 @@ void turnOffTopLEDs(); void turnOffBottomLEDs(); void runInColourSensorTestMode(); +void newHazBlockMode(); int main() { @@ -151,6 +152,8 @@ runInBreakBeamTestMode(); else if (runColourSensorTest == true) runInColourSensorTestMode(); + else if (setNewHazBlock == true) + newHazBlockMode(); if (i2cport->read_bit(11) == 1) { if (displayAbortDialog() == true) { currentMode = None; @@ -416,7 +419,9 @@ bool checkColour(int colourValues[]) { - isBetweenHazValues[3] = {false, false, false }; + for (int i = 0; i < 3; i++){ + isBetweenHazValues[i] = false; + } memset(adjustedValues, 0, sizeof(adjustedValues)); memset(percentageError, 0, sizeof(percentageError)); @@ -460,6 +465,126 @@ 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); + + 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); + 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]; + } + } + float averageComponents[4]; + memset(averageComponents, 0, sizeof(averageComponents)); + for (int i = 0; i < 4; i++){ + averageComponents[i] = ((float)totalComponents[i] / (float)readingCount); + } + float adjustedValues[3]; + memset(adjustedValues, 0, sizeof(adjustedValues)); + for (int i = 0; i < 3; i++){ + adjustedValues[i] = averageComponents[i] / averageComponents[3]; + } + + 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++){ + for (int i = 0; i < 3; i++) { + percentageError[i] = (adjustedValues[i] - kAverageValues[_HazBlock.colour][i]) / kAverageValues[_HazBlock.colour][i]; + pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[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)) { + pc.printf("DEBUG: Pass\n"); + matchesColour[i] = true; + } + if (matchesColour[0] && matchesColour[1] && matchesColour[2]){ + detectedColour = static_cast<Block::BlockColour>(k); + break; + } + } + } + + 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){ + 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();