![](/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:
- 21:60c4fe94d79c
- Parent:
- 20:4e0f0944f28f
- Child:
- 22:993821a4c396
diff -r 4e0f0944f28f -r 60c4fe94d79c main.cpp --- a/main.cpp Tue Nov 24 20:53:51 2015 +0000 +++ b/main.cpp Wed Nov 25 19:21:12 2015 +0000 @@ -28,6 +28,8 @@ Serial pc(USBTX, USBRX); uint8_t rxBuffer[kSmallBufferSize + 1]; int rxIndex = 0; +float percentageError[3]; +float adjustedValues[3]; Commander _commander = Commander(); Commander *commander = &_commander; @@ -264,30 +266,43 @@ // Cannot Abort any longer. Block is inserted. // Detach rx interrupt until block processed. NVIC_DisableIRQ(UART1_IRQn); + fpga->moveSortingServo(Haz); + fpga->moveStoppingServo(Go); + int colourValues[4]; rgbSensor.getAllColors(colourValues); - int canCheckForSize = fpga->checkForBlock(); - int blockSize = 0; + bool haz = false; + haz = checkColour(colourValues); - while (canCheckForSize == 0) { - canCheckForSize = fpga->checkForBlock(); + if (!haz) { + fpga->moveSortingServo(NonHaz); } - blockSize = fpga->checkForSize(); - bool haz = false; - - if (blockSize == HazBlock->size) { - haz = checkColour(colourValues); + while (fpga->checkForBlock() == 0) { } + int blockSize; + if (fpga->checkForSize() == HazBlock->size && haz) { +// fpga->moveSortingServo(Haz); +// fpga->moveStoppingServo(Go); + int blockSize = HazBlock->size; + while(fpga->getBeamValue(Bottom) == 1) {} + wait(kServoWait); + fpga->moveStoppingServo(Stop); } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForSize()) {} - fpga->moveStoppingServo(Stop); + haz = false; } - + fpga->moveSortingServo(NonHaz); + while(fpga->checkForSize()) {} - if (connectedToPC) - pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i,Haz:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz); + if (connectedToPC) { + for (int i = 0; i < 3; i++) { + pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]); + 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); + } // Re-Attach rx interrupt NVIC_EnableIRQ(UART1_IRQn); @@ -345,22 +360,15 @@ myLED2 = 0; myLED3 = 1; bool isHazColour[3] = {false, false, false }; - - float adjustedValues[3]; - float percentageError[3]; - + memset(adjustedValues, 0, sizeof(adjustedValues)); + memset(percentageError, 0, sizeof(percentageError)); for (int i = 0; i < 3; i++) { adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i]; - pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]); - if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0) { - pc.printf("DEBUG:%i.\n", i); + if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) { isHazColour[i] = true; - } else if (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2) { - isHazColour[i] = true; - pc.printf("DEBUG:%i.\n", i); } } @@ -373,21 +381,10 @@ bool isHazBlock = false; if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - wait(kServoWait); - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); isHazBlock = true; } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); isHazBlock = false; } - - pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], isHazBlock); return isHazBlock; }