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:
- 32:9a4046224b11
- Parent:
- 31:16e056b9f9e0
diff -r 16e056b9f9e0 -r 9a4046224b11 main.cpp --- a/main.cpp Thu Dec 03 15:50:14 2015 +0000 +++ b/main.cpp Thu Dec 17 17:23:24 2015 +0100 @@ -46,6 +46,7 @@ extern PCModes currentMode; +/// Forward declared functions void initInternal(); void initPort(int baudRate=kDefaultBaudRate); void printPCDetectedText(); @@ -62,12 +63,16 @@ void runInColourSensorTestMode(); void newHazBlockMode(); + +/// The main program function. Simply put just a infinite for loop. Not so simply put multiple infinite for loops int main() { + // Initialization initInternal(); initPort(); for (;;) { + // Show option if PC not detected. if (connectedToPC == false) { pc.printf("DEBUG: PC did not responded.\n"); lcd->cls(); @@ -79,15 +84,13 @@ lcd->printf("2: Connect to PC"); } -// wait(0.03); - int selection = 0; do { myLED4 = selection; selection = readSwitches(); } while (selection != 1 && selection != 2 && connectedToPC == false); -// lcd->cls(); - D_LEDS_OFF(); + turnOffBottomLEDs(); + if (selection == 1) { // User selected op 1: Start sorting autonomously. i2cport->write_bit(1, 12); @@ -122,6 +125,7 @@ } } + // Connect to PC if (selection == 2 || connectedToPC == true) { wait(0.1); for (;;) { @@ -129,6 +133,7 @@ i2cport->write_bit(1, 15); int abortOperation = false; + // Probe for PC since it's not already connected. if (connectedToPC == false) pc.printf(":<pc>connect;"); while (connectedToPC == false && abortOperation == false) { @@ -143,6 +148,7 @@ displayPCStatus(); while (abortOperation == false && connectedToPC == true) { + // Maintenance mode states if (currentMode == Maintanence) { displayPCStatus(); while (currentMode == Maintanence) { @@ -183,7 +189,6 @@ } else if (button == 4) { currentMode = None; pc.printf(":<mbed>mode=none;"); - // goto setModeNone; break; } } @@ -194,8 +199,8 @@ lcd->printf("Sorting mode..."); while (currentState == Start && currentMode == Normal) { if (waitForBlock() == false) { + // Sorting paused from MBED button, tell PC about it. if (connectedToPC == true && currentState != Pause && currentMode == Normal) { - // TODO: Tell PC to update UI if aborted from MBED. pc.printf(":<mbed>sort=pause;"); currentState = Pause; continue; @@ -214,7 +219,7 @@ } } else if (currentMode == None) { setModeNone: - D_LEDS_OFF(); + turnOffBottomLEDs(); i2cport->write_bit(1,15); displayPCStatus(); while (currentMode == None && abortOperation == false && connectedToPC == true) { @@ -229,7 +234,7 @@ } } } - + // Return to Main Menu if (abortOperation == true ) { connectedToPC = false; D_LEDS_OFF(); @@ -294,17 +299,20 @@ return false; } +// Sort current block; called just after block is detected. void sortBlock() { pc.printf("BLOCK: Sorting block"); myLED1 = 0; myLED2 = 1; - // Cannot Abort any longer. Block is inserted. + // Cannot Abort any longer. // Detach rx interrupt until block processed. NVIC_DisableIRQ(UART1_IRQn); + fpga->moveSortingServo(Haz); fpga->moveStoppingServo(Go); + // Take 3 readings and use the average. int colourValues[4]; int averageColourValues[4] = {0, 0, 0, 0}; int numberOfReadings = 3; @@ -320,8 +328,8 @@ lastBlockHaz = false; lastBlockHaz = checkColour(averageColourValues); -// lastBlockHaz = checkColour(colourValues); + // Tell FPGA to move servo, optimises the process. if (!lastBlockHaz) { fpga->moveSortingServo(NonHaz); } @@ -330,10 +338,8 @@ int blockSize; while (fpga->checkForBlock() == 0) { } blockSize = fpga->checkForSize(); + // Size and colour fit the criteria of the hazardous block. if (blockSize == HazBlock->size && lastBlockHaz) { - // fpga->moveSortingServo(Haz); - // fpga->moveStoppingServo(Go); - // blockSize = HazBlock->size; while(fpga->getBeamValue(Bottom) == 1) {} wait(kServoWait); fpga->moveStoppingServo(Stop); @@ -343,14 +349,12 @@ fpga->moveSortingServo(NonHaz); while(fpga->checkForSize()) {} + // Print detailed information about the block on the PC. if (connectedToPC) { for (int i = 0; i < 3; i++) { pc.printf("DEBUG:Percentage Error: %.5f. Passed?: %i\n", percentageError[i], isBetweenHazValues[i]); -// if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinRedError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxRedError[i] * errorMultiplier)) -// pc.printf("DEBUG:%i Pass.\n", i); } 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], lastBlockHaz, 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], lastBlockHaz); } // Re-Attach rx interrupt @@ -362,16 +366,18 @@ return; } +/// PC Read interrupt /// Called every-time it receives an char from PC. void Rx_interrupt() { recievingResponse = true; char interruptChar = pc.getc(); - // Uncomment to Echo to USB serial to watch data flow + // Uncomment to echo to USB serial to watch data flow // pc.putc(interruptChar); NVIC_DisableIRQ(UART1_IRQn); + // Only start parsing command if it start with three command types, otherwise ignore. if (interruptChar == CommandTypeValue[Query]) { commander->decodeCommand(Query); } else if (interruptChar == CommandTypeValue[Set]) { @@ -384,6 +390,8 @@ recievingResponse = false; } +/// Initialize internal parts. +/// Colour sensor, LCD, and Servos void initInternal() { myLED1 = 1; @@ -403,6 +411,7 @@ return; } +/// Initilise the serial port void initPort(int baudRate) { myLED3 = 1; @@ -417,6 +426,7 @@ return; } +/// Check if the passed colour values fit the hazardous block values. bool checkColour(int colourValues[]) { for (int i = 0; i < 3; i++) { @@ -425,26 +435,6 @@ memset(adjustedValues, 0, sizeof(adjustedValues)); memset(percentageError, 0, sizeof(percentageError)); - /* Harcoded working - for (int i = 0; i < 3; i++) { - adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3]; - percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i]; - - if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) { - isBetweenHazValues[i] = true; - } - } - */ - /* - 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. - 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]; @@ -465,12 +455,13 @@ return isHazBlock; } +/// Setting new hazardous block mode, from maintenance mode. 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]; @@ -487,7 +478,6 @@ 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); @@ -588,7 +578,6 @@ } while (button != 2); } - // Point and literal might not sync... _HazBlock.size = static_cast<Block::Size>(blockSize); _HazBlock.colour = detectedColour; @@ -600,6 +589,7 @@ fpga->moveSortingServo(NonHaz); } +/// Prints info on LCD about PC void printPCDetectedText() { lcd->cls(); @@ -610,6 +600,7 @@ initPort(); } +/// Return true if user aborts using the dialog. Requires confimation. bool displayAbortDialog() { while (i2cport->read_bit(11) == 1) {} @@ -627,14 +618,13 @@ D_LEDS_OFF(); if (reply == 1) { - // while (i2cport->read_bit(8)) { } return true; } else { - // while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) { } return false; } } +/// Prints servo info on the LCD in the test mode void printServoInfoOnLCD() { lcd->cls(); @@ -651,6 +641,7 @@ lcd->printf("2:Bottom: NonHaz"); } +/// Sends servo test mode info on the pc void printServoInfoOnPC() { if (fpga->stoppingServoPosition == Stop) @@ -664,6 +655,7 @@ pc.printf(":<servos>2=Haz;"); } +/// Runs the servo test mode void runInServoTestMode() { pc.printf("VALUES:Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i\n:VALUES", Stop, Go, Haz, NonHaz); @@ -717,6 +709,7 @@ return; } +/// Print break bream test mode info on the LCDs void printBeamInfoOnLCD() { lcd->cls(); @@ -726,11 +719,13 @@ lcd->printf("On:High, Off:Low"); } +/// Sends break bream test info to the PC void printBeamInfoOnPC(int topBeam, int bottomBeam) { pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam); } +/// Runs the break beam test void runInBreakBeamTestMode() { turnOffTopLEDs(); @@ -776,6 +771,7 @@ return; } +/// Prints colour sensor test info on the LCD void printColourSensorInfoOnLCD(int colourValues[]) { float weightedValues[4]; @@ -791,11 +787,13 @@ lcd->printf("Test mode"); } +/// Sends colour sensor values to the PC void printColourSensorInfoOnPC(int colourValues[]) { pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]); } +/// Runs the colour sensor test mode void runInColourSensorTestMode() { turnOffTopLEDs(); @@ -886,6 +884,7 @@ return; } +/// Displays 'Waiting...' on the LCD. Used several times so simpler to create a funciton void displayWaitingLine() { lcd->cls(); @@ -894,6 +893,7 @@ lcd->locate(1,0); } +/// Displays current PC status on the LCD. void displayPCStatus() { lcd->cls(); @@ -914,6 +914,7 @@ i2cport->write_bit(1,15); } +/// Turns off all the top LEDs void turnOffTopLEDs() { myLED1 = 0; @@ -922,6 +923,7 @@ myLED4 = 0; } +/// Turns off all the bottom swithc LEDs void turnOffBottomLEDs() { i2cport->write_bit(0, 12);