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:
- 22:993821a4c396
- Parent:
- 21:60c4fe94d79c
- Child:
- 23:db91aaa43a9e
diff -r 60c4fe94d79c -r 993821a4c396 main.cpp --- a/main.cpp Wed Nov 25 19:21:12 2015 +0000 +++ b/main.cpp Thu Nov 26 15:47:17 2015 +0000 @@ -270,21 +270,30 @@ fpga->moveStoppingServo(Go); int colourValues[4]; - rgbSensor.getAllColors(colourValues); + int averageColourValues[4] = {0, 0, 0, 0}; + for (int i = 0; i < 3; 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] / 3; + } bool haz = false; haz = checkColour(colourValues); - + if (!haz) { fpga->moveSortingServo(NonHaz); } - - while (fpga->checkForBlock() == 0) { } - int blockSize; - if (fpga->checkForSize() == HazBlock->size && haz) { + int blockSize; + while (fpga->checkForBlock() == 0) { } + blockSize = fpga->checkForSize(); + if (blockSize == HazBlock->size && haz) { // fpga->moveSortingServo(Haz); // fpga->moveStoppingServo(Go); - int blockSize = HazBlock->size; +// blockSize = HazBlock->size; while(fpga->getBeamValue(Bottom) == 1) {} wait(kServoWait); fpga->moveStoppingServo(Stop); @@ -300,8 +309,8 @@ if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) pc.printf("DEBUG:%i.\n", i); } - pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], haz); - pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz); + 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], haz, 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], haz); } // Re-Attach rx interrupt @@ -385,6 +394,7 @@ } else { isHazBlock = false; } +// pc.printf("DEBUG:haz: %i\n", isHazBlock); return isHazBlock; }