![](/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: globals.cpp
- Revision:
- 29:9c0339e3c593
- Parent:
- 28:7e4d29977d72
- Child:
- 30:c0bc92d009fe
diff -r 7e4d29977d72 -r 9c0339e3c593 globals.cpp --- a/globals.cpp Tue Dec 01 20:54:26 2015 +0000 +++ b/globals.cpp Tue Dec 01 22:56:08 2015 +0000 @@ -15,8 +15,8 @@ float currentMinError[3] = {0,0,0}; float currentMaxError[3] = {0,0,0}; -Block defaultHazBlock = Block(); -Block _HazBlock = Block(); +Block defaultHazBlock = Block(Block::Small, Block::Red); +Block _HazBlock = Block(defaultHazBlock); bool connectedToPC = false; bool runServoTest = false; @@ -24,6 +24,7 @@ bool runColourSensorTest = false; bool getColourSensorValue = false; bool getBlockColourValue = false; +bool setNewHazBlock = false; bool pcModeChanged = false; int errorMultiplier = 1; int hazReadingCount = 1; @@ -34,8 +35,8 @@ void DefaultHazBlock(){ for (int i = 0; i < 3; i++){ - currentMaxError[i] = kMaxError[i]; - currentMinError[i] = kMinError[i]; + currentMaxError[i] = kMaxRedError[i]; + currentMinError[i] = kMinRedError[i]; } Colour _minRedBlock = Colour(); Colour _maxRedBlock = Colour(); @@ -110,11 +111,17 @@ void hazBlock(CommandTypeRaw typeRaw) { if (typeRaw == Set) { +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; - char readyChar = '\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); @@ -125,7 +132,7 @@ if (readSwitches() == 4) { if (displayAbortDialog()) { //TODO: tell pc - pc.printf(":<mbed>haz-block=pause"); + pc.printf(":<mbed>haz-block=pause;"); pc.printf("INFO: Operation aborted form MBED.\n"); fpga->moveSortingServo(NonHaz); displayPCStatus(); @@ -135,50 +142,91 @@ lcd->locate(0,0); lcd->printf("New haz block"); } + } + } while (higherBeam != 1 && setNewHazBlock == true); - } - } while (higherBeam != 1); + if (setNewHazBlock == false) { + displayPCStatus(); + return; + } - 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; - } - - int colourValue[4]; - rgbSensor.getAllColors(colourValue); - do { - lowerBeam = fpga->checkForBlock(); - } while (lowerBeam != 1); - higherBeam = fpga->checkForSize(); + 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); + } + Block::Size blockSize = higherBeam; + + int totalComponents[3]; + 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[3]; + memset(averageComponents, 0, sizeof(averageComponents)); + + for (int i = 0; i < 4; i++){ + averageComponents = (float)totalComponents[i] / (float)readingCount; + } + + for (int i = 0; i < 3; i++){ + adjustedValues[i] = averageComponents[i] / averageComponents[i]; + } + + Block::BlockColour detectedColour = Block::Wrong; + bool matchesColour[3] = {false, false, false}; + + 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]; - _HazBlock = Block(); - if (higherBeam == 1) - _HazBlock.size = Block::Big; - else if (higherBeam == 0) - _HazBlock.size = Block::Small; - for (int i = 0; i < 3; i++) { - _HazBlock.minColour.components[i] = colourValue[i]/colourValue[3] - ColourSensorError; - _HazBlock.maxColour.components[i] = colourValue[i]/colourValue[3] + ColourSensorError; + 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)) { + matchesColour[i] = true; + } + if (matchesColour[0] && matchesColour[1] && matchesColour[2]){ + detectedColour = k; + break; + } + } } - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while (fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); + + 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 = 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); - - //TODO: Fix format. - pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); } else if (typeRaw == Query) { - pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]); + 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]); } }