![](/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:
- 23:db91aaa43a9e
- Parent:
- 22:993821a4c396
- Child:
- 24:02c61793f90b
--- a/main.cpp Thu Nov 26 15:47:17 2015 +0000 +++ b/main.cpp Sat Nov 28 23:28:22 2015 +0000 @@ -45,7 +45,6 @@ void initInternal(); void initPort(int baudRate=kDefaultBaudRate); void printPCDetectedText(); -//bool displayAbortDialog(); void Rx_interrupt(); bool checkColour(int colourValues[]); void runInServoTestMode(); @@ -53,454 +52,551 @@ void displayPCStatus(); bool waitForBlock(); void sortBlock(); +void runInBreakBeamTestMode(); +void turnOffTopLEDs(); +void turnOffBottomLEDs(); 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(3); + rgbSensor.enablePowerAndRGBC(); + rgbSensor.setIntegrationTime(3); - // 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); - for (;;) { - lcd->cls(); - i2cport->write_bit(1, 12); - lcd->printf("1: Start sorting."); - LCDSL(); - i2cport->write_bit(1,13); - lcd->printf("2: Connect to PC"); + for (;;) { + lcd->cls(); + i2cport->write_bit(1, 12); + lcd->printf("1: Start sorting."); + LCDSL(); + i2cport->write_bit(1,13); + lcd->printf("2: Connect to PC"); - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); + fpga->moveStoppingServo(Stop); + fpga->moveSortingServo(NonHaz); - 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); + 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."); + for(;;) { + displayWaitingLine(); + lcd->printf("for block."); - i2cport->write_bit(1, 15); + i2cport->write_bit(1, 15); - // Break and return to main menu i.e. Start Op, or Connect to PC. - if (waitForBlock() == false) { - D_LEDS_OFF(); - break; - } else { - sortBlock(); - } - } - } + // Break and return to main menu i.e. Start Op, or Connect to PC. + if (waitForBlock() == false) { + D_LEDS_OFF(); + break; + } else { + sortBlock(); + } + } + } + + if (selection == 2 || connectedToPC == true) { + for (;;) { + displayPCStatus(); - if (selection == 2 || connectedToPC == true) { - for (;;) { - displayPCStatus(); + i2cport->write_bit(1, 15); + int abortOperation = false; + if (connectedToPC == false) + pc.printf(":<pc>connect;"); + while (connectedToPC == false && abortOperation == false) { + abortOperation = readSwitches() == 4; + } - i2cport->write_bit(1, 15); - int abortOperation = false; - while (connectedToPC == false && abortOperation == false) { - abortOperation = readSwitches() == 4; - } + if (abortOperation == true) { + D_LEDS_OFF(); + break; + } - if (abortOperation == true) { - D_LEDS_OFF(); - break; - } - - displayPCStatus(); + displayPCStatus(); - while (abortOperation == false && connectedToPC == true) { - if (currentMode == Maintanence) { - if (runServoTest == true) - runInServoTestMode(); - } 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("state:start\n"); - currentState = Start; - } else if (button == 4) { - currentMode = 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) { - // TODO: Tell PC to update UI if aborted from MBED. - pc.printf("state:pause\n"); - 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) { + if (runServoTest == true) + runInServoTestMode(); + else if (runBreakBeamTest == true) + runInBreakBeamTestMode(); + } 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("state:start\n"); + currentState = Start; + } else if (button == 4) { + currentMode = 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) { + // TODO: Tell PC to update UI if aborted from MBED. + pc.printf("state:pause\n"); + 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; - } + if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence) + return false; + else + return true; + } } 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}; + 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); - } + bool haz = false; + haz = checkColour(colourValues); + + 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()) {} + 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()) {} - 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: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] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) + pc.printf("DEBUG:%i.\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); + } - // Re-Attach rx interrupt - NVIC_EnableIRQ(UART1_IRQn); - myLED3 = 0; - myLED4 = 1; - return; + // Re-Attach rx interrupt + NVIC_EnableIRQ(UART1_IRQn); + 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); + pc.printf(":<pc>connect;"); + 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)); + myLED2 = 0; + myLED3 = 1; + bool isHazColour[3] = {false, false, false }; + 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]; + 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; - } - } + 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++) { -// if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { -// 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; + // } + // } - bool isHazBlock = false; + bool isHazBlock = false; - if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { - isHazBlock = true; - } else { - isHazBlock = false; - } -// pc.printf("DEBUG:haz: %i\n", isHazBlock); - return isHazBlock; + if (isHazColour[0] && isHazColour[1] && isHazColour[2]) { + isHazBlock = true; + } else { + isHazBlock = false; + } + + 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); - lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition); - lcd->locate(1,0); - lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition); + lcd->cls(); + lcd->locate(0,0); + if (fpga->stoppingServoPosition == Stop) + lcd->printf("1: Stopping servo: Stop"); + else if (fpga->stoppingServoPosition == Go) + lcd->printf("1: Stopping servo: Go"); + + lcd->locate(1,0); + if (fpga->sortingServoPosition == NonHaz) + lcd->printf("2: Sorting servo: NonHaz"); + else if (fpga->sortingServoPosition == Haz) + lcd->printf("2: Sorting serov: Haz"); +} + +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;"); } 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); - printServoInfoOnLCD(); - int finished = false; + 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(); + 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); + + 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; +} - i2cport->write_bit(1, 12); - i2cport->write_bit(1, 13); - i2cport->write_bit(1, 15); - int button = 0; - do { - button = readSwitches(); - if (button == 1) { - fpga->toggleStoppingServo(); - printServoInfoOnLCD(); - wait(kServoWait); - } else if (button == 2) { - fpga->toggleSortingServo(); - printServoInfoOnLCD(); - wait(kServoWait); - } - finished = button == 4; - button = 0; - } while (finished == false && runServoTest != false); +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>1=%i,2=%i;", topBeam, bottomBeam); +} + +void runInBreakBeamTestMode(){ + turnOffTopLEDs(); + i2cport->write_bit(1, 15); + + int topBeamValue = fpga->getBeamValue(1); + int bottomBeamValue = fpga->getBeamValue(2); + printBeamInfoOnPC(topBeamValue, bottomBeamValue); + printBeamInfoOnLCD(); - D_LEDS_OFF(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Done servo test"); - //TODO: Inform PC of end. - wait(0.5); - lcd->cls(); - return; + int button = 0; + bool finished = false; + do{ + button = readSwitches(); + int currentTopBeamValue = fpga->getBeamValue(1); + int currentBottomBeamValue = fpga->getBeamValue(2); + myLED1 = currentTopBeamValue; + myLED2 = currentBottomBeamValue; + + 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; } 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) { + 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 turnOffBottomLEDs(){ + i2cport->write_bit(0, 12); + i2cport->write_bit(0, 13); + i2cport->write_bit(0, 14); + i2cport->write_bit(0, 15); +}