![](/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:
- 27:2cb1bdb7ae3d
- Parent:
- 26:bbcc25418ffa
- Child:
- 28:7e4d29977d72
diff -r bbcc25418ffa -r 2cb1bdb7ae3d main.cpp --- a/main.cpp Mon Nov 30 21:29:51 2015 +0000 +++ b/main.cpp Tue Dec 01 16:16:26 2015 +0000 @@ -30,6 +30,8 @@ int rxIndex = 0; float percentageError[3]; float adjustedValues[3]; +int blockCount = 0; +bool lastBlockHaz = false; Commander _commander = Commander(); Commander *commander = &_commander; @@ -59,678 +61,723 @@ int main() { - initInternal(); - initPort(); - srand((unsigned)time(NULL)); - U_LEDS_OFF(); - lcd->cls(); + initInternal(); + initPort(); + srand((unsigned)time(NULL)); + U_LEDS_OFF(); + lcd->cls(); - rgbSensor.enablePowerAndRGBC(); - rgbSensor.setIntegrationTime(gIntegrationTime); + rgbSensor.enablePowerAndRGBC(); + rgbSensor.setIntegrationTime(gIntegrationTime); - // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there. - // https://developer.mbed.org/cookbook/Serial-Interrupts - pc.attach(&Rx_interrupt, Serial::RxIrq); + // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there. + // https://developer.mbed.org/cookbook/Serial-Interrupts + pc.attach(&Rx_interrupt, Serial::RxIrq); - DefaultHazBlock(); - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); - pc.printf(":<pc>connect;"); - - for (;;) { - if (connectedToPC == false){ - lcd->cls(); - i2cport->write_bit(1, 12); - lcd->printf("1: Start sorting."); - LCDSL(); - i2cport->write_bit(1,13); - lcd->printf("2: Connect to PC"); - } - + DefaultHazBlock(); + fpga->moveStoppingServo(Stop); + fpga->moveSortingServo(NonHaz); + pc.printf(":<pc>connect;"); + wait(0.2); + + for (;;) { + if (connectedToPC == false) { + lcd->cls(); + i2cport->write_bit(1, 12); + lcd->locate(0,0); + lcd->printf("1: Start sorting."); + lcd->locate(1,0); + i2cport->write_bit(1,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); - D_LEDS_OFF(); - if (selection == 1) { - // User selected op 1: Start sorting autonomously. - i2cport->write_bit(1, 12); - lcd->cls(); - LCDFL(); - lcd->printf("Starting sorting"); - wait(0.5); - D_LEDS_OFF(); + int selection = 0; + do { + myLED4 = selection; + selection = readSwitches(); + } while (selection != 1 && selection != 2 && connectedToPC == false); +// lcd->cls(); + D_LEDS_OFF(); + if (selection == 1) { + // User selected op 1: Start sorting autonomously. + i2cport->write_bit(1, 12); + lcd->cls(); + LCDFL(); + lcd->printf("Starting sorting"); + wait(0.5); + D_LEDS_OFF(); - for(;;) { - displayWaitingLine(); - lcd->printf("for block."); - - i2cport->write_bit(1, 15); + for(;;) { + if (blockCount > 0) { + lcd->cls(); + displayWaitingLine(); + lcd->locate(1,0); + if (lastBlockHaz == true) + lcd->printf("Hazardous"); + else if (lastBlockHaz == false) + lcd->printf("Non-Hazardous"); + } else { + displayWaitingLine(); + } - // Break and return to main menu i.e. Start Op, or Connect to PC. - if (waitForBlock() == false) { - D_LEDS_OFF(); - break; - } else { - sortBlock(); - } - } - } + i2cport->write_bit(1, 15); - if (selection == 2 || connectedToPC == true) { - for (;;) { - displayPCStatus(); + // Break and return to main menu i.e. Start Op, or Connect to PC. + if (waitForBlock() == false) { + D_LEDS_OFF(); + break; + } else { + sortBlock(); + } + } + } - i2cport->write_bit(1, 15); - int abortOperation = false; - if (connectedToPC == false) - pc.printf(":<pc>connect;"); - while (connectedToPC == false && abortOperation == false) { - abortOperation = readSwitches() == 4; - } + if (selection == 2 || connectedToPC == true) { + wait(0.1); + for (;;) { + displayPCStatus(); - if (abortOperation == true) { - D_LEDS_OFF(); - break; - } + i2cport->write_bit(1, 15); + int abortOperation = false; + if (connectedToPC == false) + pc.printf(":<pc>connect;"); + while (connectedToPC == false && abortOperation == false) { + abortOperation = readSwitches() == 4; + } - displayPCStatus(); + if (abortOperation == true) { + D_LEDS_OFF(); + break; + } + + displayPCStatus(); - while (abortOperation == false && connectedToPC == true) { - if (currentMode == Maintanence) { - displayPCStatus(); - while (currentMode == Maintanence){ - if (runServoTest == true) - runInServoTestMode(); - else if (runBreakBeamTest == true) - runInBreakBeamTestMode(); - else if (runColourSensorTest == true) - runInColourSensorTestMode(); - if (i2cport->read_bit(11) == 1){ - if (displayAbortDialog() == true){ - currentMode = None; - pc.printf(":<mbed>mode=none;"); - }else { - displayPCStatus(); - } - } - } - } else if (currentMode == Normal) { - displayPCStatus(); - while (currentMode == Normal) { - if (currentState == Pause) { - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Sorting Paused."); - lcd->locate(1,0); - lcd->printf("1: Start"); - int button = 0; - i2cport->write_bit(1, 12); - i2cport->write_bit(1, 15); - while (currentState == Pause && currentMode == Normal) { - button = readSwitches(); - if (button == 1) { - pc.printf(":<mbed>sort=start;"); - currentState = Start; - } else if (button == 4) { - currentMode = None; - pc.printf(":<mbed>mode=none;"); - // goto setModeNone; - break; - } - } - } - if (currentState == Start) { - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Sorting mode..."); - while (currentState == Start && currentMode == Normal) { - if (waitForBlock() == false) { - if (connectedToPC == true && currentState != Pause) { - // TODO: Tell PC to update UI if aborted from MBED. - pc.printf(":<mbed>sort=pause;"); - currentState = Pause; - continue; - } else if (connectedToPC == false) { - currentMode = None; - abortOperation = true; - } - } else { - sortBlock(); - } - } - } - } - } else if (currentMode == None) { - setModeNone: - D_LEDS_OFF(); - i2cport->write_bit(1,15); - displayPCStatus(); - while (currentMode == None && abortOperation == false && connectedToPC == true) { - if (i2cport->read_bit(11)) { - abortOperation = displayAbortDialog(); - // Cancel the Abort - if (abortOperation == false) { - displayPCStatus(); - i2cport->write_bit(1, 15); - } - } - } - } - } + while (abortOperation == false && connectedToPC == true) { + if (currentMode == Maintanence) { + displayPCStatus(); + while (currentMode == Maintanence) { + if (runServoTest == true) + runInServoTestMode(); + else if (runBreakBeamTest == true) + runInBreakBeamTestMode(); + else if (runColourSensorTest == true) + runInColourSensorTestMode(); + if (i2cport->read_bit(11) == 1) { + if (displayAbortDialog() == true) { + currentMode = None; + pc.printf(":<mbed>mode=none;"); + } else { + displayPCStatus(); + } + } + } + } else if (currentMode == Normal) { + displayPCStatus(); + while (currentMode == Normal) { + if (currentState == Pause) { + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Sorting Paused."); + lcd->locate(1,0); + lcd->printf("1: Start"); + int button = 0; + i2cport->write_bit(1, 12); + i2cport->write_bit(1, 15); + while (currentState == Pause && currentMode == Normal) { + button = readSwitches(); + if (button == 1) { + pc.printf(":<mbed>sort=start;"); + currentState = Start; + } else if (button == 4) { + currentMode = None; + pc.printf(":<mbed>mode=none;"); + // goto setModeNone; + break; + } + } + } + if (currentState == Start) { + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Sorting mode..."); + while (currentState == Start && currentMode == Normal) { + if (waitForBlock() == false) { + if (connectedToPC == true && currentState != Pause) { + // TODO: Tell PC to update UI if aborted from MBED. + pc.printf(":<mbed>sort=pause;"); + currentState = Pause; + continue; + } else if (connectedToPC == false) { + currentMode = None; + abortOperation = true; + } + } else { + sortBlock(); + } + } + } + } + } else if (currentMode == None) { +setModeNone: + D_LEDS_OFF(); + i2cport->write_bit(1,15); + displayPCStatus(); + while (currentMode == None && abortOperation == false && connectedToPC == true) { + if (i2cport->read_bit(11)) { + abortOperation = displayAbortDialog(); + // Cancel the Abort + if (abortOperation == false) { + displayPCStatus(); + i2cport->write_bit(1, 15); + } + } + } + } + } - if (abortOperation == true ) { - connectedToPC = false; - D_LEDS_OFF(); - break; - } - } - } + if (abortOperation == true ) { + connectedToPC = false; + D_LEDS_OFF(); + break; + } + } + } - } + } } // Waits until detects block. // true if block detected, false if cancelled/connected to PC. bool waitForBlock() { - myLED4 = 0; - myLED1 = 1; - if (connectedToPC == false) { - bool abortOperation = false; - int blockInserted = 0; - // Wait until a block is breaking the beam, or button 4 is pressed to abort. - do { - blockInserted = fpga->getBeamValue(Top); - myLED4 = blockInserted; - if (i2cport->read_bit(11)) { - abortOperation = displayAbortDialog(); - // Cancel the Abort - if (abortOperation == false) { - displayWaitingLine(); - lcd->printf("for block"); - i2cport->write_bit(1, 15); - } - } - } while (abortOperation == false && blockInserted != 1 && connectedToPC == false); + myLED4 = 0; + myLED1 = 1; + if (connectedToPC == false) { + bool abortOperation = false; + int blockInserted = 0; + // Wait until a block is breaking the beam, or button 4 is pressed to abort. + do { + blockInserted = fpga->getBeamValue(Top); + myLED4 = blockInserted; + if (i2cport->read_bit(11)) { + abortOperation = displayAbortDialog(); + // Cancel the Abort + if (abortOperation == false) { + displayWaitingLine(); + lcd->printf("for block"); + i2cport->write_bit(1, 15); + } + } + } while (abortOperation == false && blockInserted != 1 && connectedToPC == false); - if (abortOperation == true || connectedToPC == true) - return false; - else - return true; - } else if (connectedToPC == true) { - bool abortOperation = false; - int blockInserted = 0; - // Wait until a block is breaking the beam, or button 4 is pressed to abort. - do { - blockInserted = fpga->getBeamValue(Top); - myLED4 = blockInserted; - if (i2cport->read_bit(11)) { - abortOperation = displayAbortDialog(); - // Cancel the Abort - if (abortOperation == false) { - displayWaitingLine(); - lcd->printf("for block"); - i2cport->write_bit(1, 15); - } - } - } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal); + if (abortOperation == true || connectedToPC == true) + return false; + else + return true; + } else if (connectedToPC == true) { + bool abortOperation = false; + int blockInserted = 0; + // Wait until a block is breaking the beam, or button 4 is pressed to abort. + do { + blockInserted = fpga->getBeamValue(Top); + myLED4 = blockInserted; + if (i2cport->read_bit(11)) { + abortOperation = displayAbortDialog(); + // Cancel the Abort + if (abortOperation == false) { + displayWaitingLine(); + lcd->printf("for block"); + i2cport->write_bit(1, 15); + } + } + } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal); - if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence) - return false; - else - return true; - } - return false; + if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence) + return false; + else + return true; + } + return false; } void sortBlock() { - myLED1 = 0; - myLED2 = 1; - // Cannot Abort any longer. Block is inserted. - // Detach rx interrupt until block processed. - NVIC_DisableIRQ(UART1_IRQn); - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); + myLED1 = 0; + myLED2 = 1; + // 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]; - 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; - } + 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; + } - bool haz = false; - haz = checkColour(colourValues); + lastBlockHaz = false; + lastBlockHaz = checkColour(averageColourValues); + + if (!lastBlockHaz) { + fpga->moveSortingServo(NonHaz); + } - if (!haz) { - fpga->moveSortingServo(NonHaz); - } - int blockSize; - while (fpga->checkForBlock() == 0) { } - blockSize = fpga->checkForSize(); - if (blockSize == HazBlock->size && haz) { - // fpga->moveSortingServo(Haz); - // fpga->moveStoppingServo(Go); - // blockSize = HazBlock->size; - while(fpga->getBeamValue(Bottom) == 1) {} - wait(kServoWait); - fpga->moveStoppingServo(Stop); - } else { - haz = false; - } - fpga->moveSortingServo(NonHaz); - while(fpga->checkForSize()) {} + myLED3 = 1; + int blockSize; + while (fpga->checkForBlock() == 0) { } + blockSize = fpga->checkForSize(); + if (blockSize == HazBlock->size && lastBlockHaz) { + // fpga->moveSortingServo(Haz); + // fpga->moveStoppingServo(Go); + // blockSize = HazBlock->size; + while(fpga->getBeamValue(Bottom) == 1) {} + wait(kServoWait); + fpga->moveStoppingServo(Stop); + } else { + lastBlockHaz = false; + } + fpga->moveSortingServo(NonHaz); + while(fpga->checkForSize()) {} - 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 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], 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); - } + 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] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[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 - NVIC_EnableIRQ(UART1_IRQn); - myLED3 = 0; - myLED4 = 1; - return; + // Re-Attach rx interrupt + NVIC_EnableIRQ(UART1_IRQn); + + blockCount++; + myLED3 = 0; + myLED4 = 1; + return; } /// Called every-time it receives an char from PC. void Rx_interrupt() { - char interruptChar = pc.getc(); - // Uncomment to Echo to USB serial to watch data flow - // pc.putc(interruptChar); + char interruptChar = pc.getc(); + // Uncomment to Echo to USB serial to watch data flow + // pc.putc(interruptChar); - NVIC_DisableIRQ(UART1_IRQn); + NVIC_DisableIRQ(UART1_IRQn); - if (interruptChar == CommandTypeValue[Query]) { - commander->decodeCommand(Query); - } else if (interruptChar == CommandTypeValue[Set]) { - commander->decodeCommand(Set); - } else if (interruptChar== CommandTypeValue[Reply]) { - commander->decodeCommand(Reply); - } + if (interruptChar == CommandTypeValue[Query]) { + commander->decodeCommand(Query); + } else if (interruptChar == CommandTypeValue[Set]) { + commander->decodeCommand(Set); + } else if (interruptChar== CommandTypeValue[Reply]) { + commander->decodeCommand(Reply); + } - NVIC_EnableIRQ(UART1_IRQn); + NVIC_EnableIRQ(UART1_IRQn); } void initInternal() { - myLED1 = 1; - i2cport = new MCP23017(p9, p10, 0x40); - lcd = new WattBob_TextLCD(i2cport); - BACKLIGHT_ON(i2cport); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Initilizing..."); - DefaultHazBlock(); - myLED2 = 1; - return; + myLED1 = 1; + i2cport = new MCP23017(p9, p10, 0x40); + lcd = new WattBob_TextLCD(i2cport); + BACKLIGHT_ON(i2cport); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Initilizing..."); + DefaultHazBlock(); + myLED2 = 1; + return; } void initPort(int baudRate) { - myLED3 = 1; - pc.baud(baudRate); - pc.format(8, SerialBase::None, gStopBits); - myLED4 = 1; - wait (0.1); - return; + myLED3 = 1; + pc.baud(baudRate); + pc.format(8, SerialBase::None, gStopBits); + myLED4 = 1; + wait (0.1); + return; } bool checkColour(int colourValues[]) { - myLED2 = 0; - myLED3 = 1; - bool isHazColour[3] = {false, false, false }; - memset(adjustedValues, 0, sizeof(adjustedValues)); - memset(percentageError, 0, sizeof(percentageError)); + bool isHazColour[3] = {false, false, false }; + 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]; -/* 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)) { - isHazColour[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]; + 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; + } + } + */ + 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]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) { - isHazColour[i] = true; - } - } - - // for (int i = 0; i < 3; i++) { - // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { - // isHazColour[i] = true; - // } - // } + 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; + } + } - bool isHazBlock = false; + // for (int i = 0; i < 3; i++) { + // if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { + // isHazColour[i] = true; + // } + // } + + bool isHazBlock = false; - if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { - isHazBlock = true; - } else { - isHazBlock = false; - } - - return isHazBlock; + if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { + isHazBlock = true; + } else { + isHazBlock = false; + } + myLED2 = 0; + return isHazBlock; } void printPCDetectedText() { - lcd->cls(); - LCDFL(); - lcd->printf("Detected PC."); - LCDSL(); - lcd->printf("Connecting"); - initPort(); + lcd->cls(); + LCDFL(); + lcd->printf("Detected PC."); + LCDSL(); + lcd->printf("Connecting"); + initPort(); } bool displayAbortDialog() { - while (i2cport->read_bit(11) == 1) {} - i2cport->write_bit(1, 12); + while (i2cport->read_bit(11) == 1) {} + i2cport->write_bit(1, 12); - lcd->cls(); - LCDFL(); - lcd->printf("Abort?"); - LCDSL(); - lcd->printf("1:Yes, 2,3,4:No"); - int reply = 0; - do { - reply = readSwitches(); - } while(reply == 0); + lcd->cls(); + LCDFL(); + lcd->printf("Abort?"); + LCDSL(); + lcd->printf("1:Yes, 2,3,4:No"); + int reply = 0; + do { + reply = readSwitches(); + } while(reply == 0); - 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; - } + 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; + } } void printServoInfoOnLCD() { - lcd->cls(); - lcd->locate(0,0); - if (fpga->stoppingServoPosition == Stop) - lcd->printf("1:Top: Go"); - else if (fpga->stoppingServoPosition == Go) - lcd->printf("1:Top: Stop"); + lcd->cls(); + lcd->locate(0,0); + if (fpga->stoppingServoPosition == Stop) + lcd->printf("1:Top: Go"); + else if (fpga->stoppingServoPosition == Go) + lcd->printf("1:Top: Stop"); - lcd->locate(1,0); - if (fpga->sortingServoPosition == NonHaz) - lcd->printf("2:Bottom: Haz"); - else if (fpga->sortingServoPosition == Haz) - lcd->printf("2:Bottom: NonHaz"); + lcd->locate(1,0); + if (fpga->sortingServoPosition == NonHaz) + lcd->printf("2:Bottom: Haz"); + else if (fpga->sortingServoPosition == Haz) + lcd->printf("2:Bottom: NonHaz"); } -void printServoInfoOnPC() { - if (fpga->stoppingServoPosition == Stop) - pc.printf(":<servos>1=Stop;"); - else if (fpga->stoppingServoPosition == Go) - pc.printf(":<servos>1=Go;"); +void printServoInfoOnPC() +{ + if (fpga->stoppingServoPosition == Stop) + pc.printf(":<servos>1=Stop;"); + else if (fpga->stoppingServoPosition == Go) + pc.printf(":<servos>1=Go;"); - if (fpga->sortingServoPosition == NonHaz) - pc.printf(":<servos>2=NonHaz;"); - else if (fpga->sortingServoPosition == Haz) - pc.printf(":<servos>2=Haz;"); + if (fpga->sortingServoPosition == NonHaz) + pc.printf(":<servos>2=NonHaz;"); + else if (fpga->sortingServoPosition == Haz) + pc.printf(":<servos>2=Haz;"); } 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); - printServoInfoOnPC(); - printServoInfoOnLCD(); + 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); + printServoInfoOnPC(); + printServoInfoOnLCD(); + + i2cport->write_bit(1, 12); + i2cport->write_bit(1, 13); + i2cport->write_bit(1, 15); + int button = 0; + bool finished = false; + do { + button = readSwitches(); + + // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos + if (gToggleServoNumber == 1) { + button = 1; + gToggleServoNumber = 0; + } else if (gToggleServoNumber == 2) { + button = 2; + gToggleServoNumber = 0; + } - i2cport->write_bit(1, 12); - i2cport->write_bit(1, 13); - i2cport->write_bit(1, 15); - int button = 0; - bool finished = false; - do { - button = readSwitches(); - - // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos - if (gToggleServoNumber == 1){ - button = 1; - gToggleServoNumber = 0; - }else if (gToggleServoNumber == 2){ - button = 2; - gToggleServoNumber = 0; - } - - if (button == 1) { - fpga->toggleStoppingServo(); - printServoInfoOnLCD(); - printServoInfoOnPC(); - wait(kServoWait); - } else if (button == 2) { - fpga->toggleSortingServo(); - printServoInfoOnLCD(); - printServoInfoOnPC(); - wait(kServoWait); - } - - finished = button == 4; - button = 0; - } while (finished == false && runServoTest == true); + if (button == 1) { + fpga->toggleStoppingServo(); + printServoInfoOnLCD(); + printServoInfoOnPC(); + wait(kServoWait); + } else if (button == 2) { + fpga->toggleSortingServo(); + printServoInfoOnLCD(); + printServoInfoOnPC(); + wait(kServoWait); + } - D_LEDS_OFF(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Done servo test"); - if (runServoTest == true){ - pc.printf(":<servos>test=pause;"); - runServoTest = false; - } - wait(0.5); - lcd->cls(); - return; + finished = button == 4; + button = 0; + } while (finished == false && runServoTest == true); + + D_LEDS_OFF(); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Done servo test"); + if (runServoTest == true) { + pc.printf(":<servos>test=pause;"); + runServoTest = false; + } + wait(0.5); + lcd->cls(); + return; } -void printBeamInfoOnLCD(){ - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Top:L1 Btm: L2"); - lcd->locate(1,0); - lcd->printf("On:High, Off:Low"); +void printBeamInfoOnLCD() +{ + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Top:L1 Btm: L2"); + lcd->locate(1,0); + lcd->printf("On:High, Off:Low"); } -void printBeamInfoOnPC(int topBeam, int bottomBeam){ - pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam); +void printBeamInfoOnPC(int topBeam, int bottomBeam) +{ + pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam); } -void runInBreakBeamTestMode(){ - turnOffTopLEDs(); - i2cport->write_bit(1, 15); +void runInBreakBeamTestMode() +{ + turnOffTopLEDs(); + i2cport->write_bit(1, 15); - int topBeamValue = fpga->getBeamValue(1); - int bottomBeamValue = fpga->getBeamValue(2); - printBeamInfoOnPC(topBeamValue, bottomBeamValue); - printBeamInfoOnLCD(); + int topBeamValue = fpga->getBeamValue(1); + int bottomBeamValue = fpga->getBeamValue(2); + printBeamInfoOnPC(topBeamValue, bottomBeamValue); + printBeamInfoOnLCD(); + + int button = 0; + bool finished = false; + do { + button = readSwitches(); + int currentTopBeamValue = fpga->getBeamValue(1); + int currentBottomBeamValue = fpga->getBeamValue(2); - int button = 0; - bool finished = false; - do{ - button = readSwitches(); - int currentTopBeamValue = fpga->getBeamValue(1); - int currentBottomBeamValue = fpga->getBeamValue(2); - - // For debugging, hold down both 1 and 3 or 2 and 3 - if (i2cport->read_bit(10) == 1){ - currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10); - currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10); - } - myLED1 = currentTopBeamValue; - myLED2 = currentBottomBeamValue; + // For debugging, hold down both 1 and 3 or 2 and 3 + if (i2cport->read_bit(10) == 1) { + currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10); + currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10); + } + myLED1 = currentTopBeamValue; + myLED2 = currentBottomBeamValue; - if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ){ - topBeamValue = currentTopBeamValue; - bottomBeamValue = currentBottomBeamValue; - printBeamInfoOnPC(topBeamValue, bottomBeamValue); - } - finished = button == 4; - }while (runBreakBeamTest == true && finished == false); + if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ) { + topBeamValue = currentTopBeamValue; + bottomBeamValue = currentBottomBeamValue; + printBeamInfoOnPC(topBeamValue, bottomBeamValue); + } + finished = button == 4; + } while (runBreakBeamTest == true && finished == false); - turnOffBottomLEDs(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Finished test"); - if (runBreakBeamTest == true){ - pc.printf(":<break_beam>test=pause;"); - runBreakBeamTest = false; - } - wait(0.5); - return; + turnOffBottomLEDs(); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Finished test"); + if (runBreakBeamTest == true) { + pc.printf(":<break_beam>test=pause;"); + runBreakBeamTest = false; + } + wait(0.5); + return; } -void printColourSensorInfoOnLCD(int colourValues[]){ - float weightedValues[4]; - - for (int i = 0; i < 3; i++){ - weightedValues[i] = (float)colourValues[i] / (float)colourValues[3]; - } - //TODO: Print values on LCD - lcd->cls(); +void printColourSensorInfoOnLCD(int colourValues[]) +{ + float weightedValues[4]; + + for (int i = 0; i < 3; i++) { + weightedValues[i] = (float)colourValues[i] / (float)colourValues[3]; + } + //TODO: Print values on LCD + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Colour sensor"); + lcd->locate(1,0); + lcd->printf("Test mode"); } -void printColourSensorInfoOnPC(int colourValues[]){ - pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]); +void printColourSensorInfoOnPC(int colourValues[]) +{ + pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]); } -void runInColourSensorTestMode() { - turnOffTopLEDs(); - i2cport->write_bit(1, 15); - i2cport->write_bit(1, 12); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Colour Sensor"); - lcd->locate(1,0); - lcd->printf("Test Mode"); - pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime); - - int button = 0; - bool finished = false; - - do { - button = readSwitches(); - - if (getColourSensorValue == true){ - int colourValues[4]; - rgbSensor.getAllColors(colourValues); - printColourSensorInfoOnPC(colourValues); - printColourSensorInfoOnLCD(colourValues); - getColourSensorValue = false; - } else if (getBlockColourValue == true){ - int topBeam = fpga->getBeamValue(1); - - while(topBeam == 0){ - topBeam = fpga->getBeamValue(1); - } - int colourValues[3][4]; - for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); } - int valueCount = 0; - do { - rgbSensor.getAllColors(colourValues[valueCount]); - } while (topBeam == 1 && valueCount < 3); - int averageValues[4] = {0, 0, 0, 0}; - for (int i = 0; i < 3; i++) { - averageValues[0] += colourValues[i][0]; - averageValues[1] += colourValues[i][1]; - averageValues[2] += colourValues[i][2]; - averageValues[3] += colourValues[i][3]; - } - averageValues[0] = averageValues[0] / valueCount; - averageValues[1] = averageValues[1] / valueCount; - averageValues[2] = averageValues[2] / valueCount; - averageValues[3] = averageValues[3] / valueCount; - - printColourSensorInfoOnPC(averageValues); - printColourSensorInfoOnLCD(averageValues); - - getBlockColourValue = false; - } else if (button == 1){ - getColourSensorValue = true; - button = 0; - } - - finished = button == 4; +void runInColourSensorTestMode() +{ + turnOffTopLEDs(); + i2cport->write_bit(1, 15); + i2cport->write_bit(1, 12); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Colour Sensor"); + lcd->locate(1,0); + lcd->printf("Test Mode"); + pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime); + + int button = 0; + bool finished = false; + + do { + button = readSwitches(); + + if (getColourSensorValue == true) { + int colourValues[4]; + rgbSensor.getAllColors(colourValues); + printColourSensorInfoOnPC(colourValues); + printColourSensorInfoOnLCD(colourValues); + getColourSensorValue = false; + } else if (getBlockColourValue == true) { + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Drop block"); + + + turnOffTopLEDs(); + + while(fpga->getBeamValue(Top) == 0) { + myLED1 = fpga->getBeamValue(Top); + myLED2 = fpga->getBeamValue(Bottom); + } + + myLED3 = 1; + int colourValues[3][4]; + for (int i = 0; i < 3; i++) { + memset(colourValues[i], 0, sizeof(colourValues)); + } + int valueCount = 0; + + do { + rgbSensor.getAllColors(colourValues[valueCount]); + } while (fpga->getBeamValue(Top) == 1 && valueCount < 3); + + turnOffTopLEDs(); + myLED4 = 1; + + int averageValues[4] = {0, 0, 0, 0}; + for (int i = 0; i < 3; i++) { + averageValues[0] += colourValues[i][0]; + averageValues[1] += colourValues[i][1]; + averageValues[2] += colourValues[i][2]; + averageValues[3] += colourValues[i][3]; + } + averageValues[0] = averageValues[0] / valueCount; + averageValues[1] = averageValues[1] / valueCount; + averageValues[2] = averageValues[2] / valueCount; + averageValues[3] = averageValues[3] / valueCount; + myLED4 = 0; + + printColourSensorInfoOnPC(averageValues); + printColourSensorInfoOnLCD(averageValues); + + getBlockColourValue = false; + } else if (button == 1) { + getColourSensorValue = true; + button = 0; + } + + finished = button == 4; // button = 0; - } while (finished == false && runColourSensorTest == true); - - turnOffBottomLEDs(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Finished test"); - if (runColourSensorTest == true){ - pc.printf(":<colour_sensor>test=pause;"); - runColourSensorTest = false; - } - wait(0.5); - return; + } while (finished == false && runColourSensorTest == true); + + turnOffBottomLEDs(); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Finished test"); + if (runColourSensorTest == true) { + pc.printf(":<colour_sensor>test=pause;"); + runColourSensorTest = false; + } + wait(0.5); + return; } void displayWaitingLine() { - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Waiting..."); - lcd->locate(1,0); + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Waiting..."); + lcd->locate(1,0); } void displayPCStatus() { - lcd->cls(); - lcd->locate(0,0); - if (connectedToPC) { - if (currentMode == Normal) - lcd->printf("Normal mode."); - else if (currentMode == Maintanence) - lcd->printf("Maintanence."); - else if (currentMode == None) - lcd->printf("Connected to PC"); - } else - lcd->printf("Waiting for PC.."); + lcd->cls(); + lcd->locate(0,0); + if (connectedToPC == true) { + if (currentMode == Normal) + lcd->printf("Normal mode."); + else if (currentMode == Maintanence) + lcd->printf("Maintanence."); + else if (currentMode == None) + lcd->printf("Connected to PC"); + } else + lcd->printf("Waiting for PC.."); - lcd->locate(1,0); - lcd->printf("4:Disconnect"); - D_LEDS_OFF(); - i2cport->write_bit(1,15); + lcd->locate(1,0); + lcd->printf("4:Disconnect"); + D_LEDS_OFF(); + i2cport->write_bit(1,15); } -void turnOffTopLEDs(){ - myLED1 = 0; - myLED2 = 0; - myLED3 = 0; - myLED4 = 0; +void turnOffTopLEDs() +{ + myLED1 = 0; + myLED2 = 0; + myLED3 = 0; + myLED4 = 0; } -void turnOffBottomLEDs(){ - i2cport->write_bit(0, 12); - i2cport->write_bit(0, 13); - i2cport->write_bit(0, 14); - i2cport->write_bit(0, 15); +void turnOffBottomLEDs() +{ + i2cport->write_bit(0, 12); + i2cport->write_bit(0, 13); + i2cport->write_bit(0, 14); + i2cport->write_bit(0, 15); }