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:
- 31:16e056b9f9e0
- Parent:
- 30:c0bc92d009fe
- Child:
- 32:9a4046224b11
diff -r c0bc92d009fe -r 16e056b9f9e0 main.cpp --- a/main.cpp Tue Dec 01 23:50:29 2015 +0000 +++ b/main.cpp Thu Dec 03 15:50:14 2015 +0000 @@ -153,7 +153,7 @@ else if (runColourSensorTest == true) runInColourSensorTestMode(); else if (setNewHazBlock == true) - newHazBlockMode(); + newHazBlockMode(); if (i2cport->read_bit(11) == 1) { if (displayAbortDialog() == true) { currentMode = None; @@ -199,9 +199,9 @@ pc.printf(":<mbed>sort=pause;"); currentState = Pause; continue; - }else if (connectedToPC == true && currentMode == None) { - goto setModeNone; - }else if (connectedToPC == false) { + } else if (connectedToPC == true && currentMode == None) { + goto setModeNone; + } else if (connectedToPC == false) { currentMode = None; currentState = Pause; abortOperation = true; @@ -409,7 +409,7 @@ pc.baud(baudRate); pc.format(8, SerialBase::None, gStopBits); pc.attach(&Rx_interrupt, Serial::RxIrq); - myLED4 = 1; + myLED4 = 1; pc.printf(":<pc>connect;"); // wait (0.3); // while(recievingResponse == true) {} @@ -419,8 +419,8 @@ bool checkColour(int colourValues[]) { - for (int i = 0; i < 3; i++){ - isBetweenHazValues[i] = false; + for (int i = 0; i < 3; i++) { + isBetweenHazValues[i] = false; } memset(adjustedValues, 0, sizeof(adjustedValues)); memset(percentageError, 0, sizeof(percentageError)); @@ -439,20 +439,20 @@ 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. + // 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]; + 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; - } + 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; @@ -465,124 +465,139 @@ return isHazBlock; } -void newHazBlockMode(){ - trySetHazBlockAgain: - pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour); - fpga->moveSortingServo(Haz); +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; + 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"); - 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"); - } + 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); + } 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); - if (lowerBeam != 1){ - lowerBeam = fpga->getBeamValue(Top); - while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); } - higherBeam = fpga->getBeamValue(Bottom); + 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); } - int blockSize = higherBeam; + 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]; - } + 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]; - } + } + 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]); - 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 ((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){ - goto trySetHazBlockAgain; - } - } while (button != 2); - } + 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; + // 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("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); + 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()