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:
- 30:c0bc92d009fe
- Parent:
- 29:9c0339e3c593
- Child:
- 32:9a4046224b11
diff -r 9c0339e3c593 -r c0bc92d009fe globals.cpp --- a/globals.cpp Tue Dec 01 22:56:08 2015 +0000 +++ b/globals.cpp Tue Dec 01 23:50:29 2015 +0000 @@ -111,122 +111,11 @@ 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; - 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); - } - 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]; - - 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; - } - } - } - - 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); - } else if (typeRaw == Query) { - 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]); + setNewHazBlock = true; + pc.printf("INFO: Setting new haz block.\n"); + } else if (typeRaw == Query) { + pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%.3f, %.3f, %.3f\n \t Max Error:%.3f, %.3f, %.3f\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]); } } @@ -239,7 +128,7 @@ { gIntegrationTime = integrationTime; rgbSensor.setIntegrationTime(gIntegrationTime); - pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime); + pc.printf("DEBUG: Setting integration-time to %.2f.\n", gIntegrationTime); } void previewOnPC(bool on)