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:
- 19:61b21ac4896e
- Parent:
- 18:44a1c1a30166
- Child:
- 20:4e0f0944f28f
diff -r 44a1c1a30166 -r 61b21ac4896e main.cpp --- a/main.cpp Tue Nov 24 00:24:39 2015 +0000 +++ b/main.cpp Tue Nov 24 01:38:46 2015 +0000 @@ -54,383 +54,411 @@ int main() { - initInternal(); - initPort(); - srand((unsigned)time(NULL)); - U_LEDS_OFF(); - lcd->cls(); - myLED1 = 1; + initInternal(); + initPort(); + srand((unsigned)time(NULL)); + U_LEDS_OFF(); + lcd->cls(); + myLED1 = 1; - rgbSensor.enablePowerAndRGBC(); - rgbSensor.setIntegrationTime(2.4); + rgbSensor.enablePowerAndRGBC(); + rgbSensor.setIntegrationTime(2.4); - // 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); - waitForBlock(&abortOpration); + 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; + 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; - while (currentState == Pause){ - button = readSwitches(); - if (button == 1){ - pc.printf("state:start\n"); - currentState = Start; - }else if (button == 4){ - goto setModeNone; - } - } - } - if (currentState == Start){ - while (currentState == Start){ - if (waitForBlock() == false){ - // TODO: Tell PC to update UI if aborted from MBED. - pc.printf("state:pause\n"); - currentState = Pause; - continue; - }else { - sortBlock(); - } - } - } - } - }else if (currentMode == None){ - setModeNone: - displayPCStatus(); - while (currentMode == None) {} - } -// if (readSwitches() == 4) { -// abortOperation = displayAbortDialog(); -// if (abortOperation == false) -// 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; + while (currentState == Pause) { + button = readSwitches(); + if (button == 1) { + pc.printf("state:start\n"); + currentState = Start; + } else if (button == 4) { + goto setModeNone; + } + } + } + if (currentState == Start) { + while (currentState == Start) { + if (waitForBlock() == false) { + currentState = Pause; + if (connectedToPC == true) { + // TODO: Tell PC to update UI if aborted from MBED. + pc.printf("state:pause\n"); - if (abortOperation == true ) { - connectedToPC = false; - D_LEDS_OFF(); - break; - } - } - } + continue; + } else if (connectedToPC == false) { + currentMode = None; + abortOperation = true; + } + } else { + sortBlock(); + } + } + } + } + } else if (currentMode == None) { +setModeNone: + displayPCStatus(); + while (currentMode == None && abortOperation == false) { + abortOperation = readSwitches() == 4; + } + } + } - } + 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(){ - 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); +bool waitForBlock() +{ + 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; + 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); + + if (abortOperation == true || connectedToPC == false) + return false; + else + return true; + } } -void sortBlock(){ - // Cannot Abort any longer. Block is inserted. - // Detach rx interrupt until block processed. - NVIC_DisableIRQ(UART1_IRQn); - int colourValues[4]; - rgbSensor.getAllColors(colourValues); +void sortBlock() +{ + // Cannot Abort any longer. Block is inserted. + // Detach rx interrupt until block processed. + NVIC_DisableIRQ(UART1_IRQn); + int colourValues[4]; + rgbSensor.getAllColors(colourValues); - int canCheckForSize = fpga->checkForBlock(); - int blockSize = 0; + int canCheckForSize = fpga->checkForBlock(); + int blockSize = 0; - while (canCheckForSize == 0) { - canCheckForSize = fpga->checkForBlock(); - } + while (canCheckForSize == 0) { + canCheckForSize = fpga->checkForBlock(); + } - blockSize = fpga->checkForSize(); - myLED3 = blockSize; - bool haz = false; + blockSize = fpga->checkForSize(); + myLED3 = blockSize; + bool haz = false; - if (blockSize == HazBlock->size) { - haz = checkColour(colourValues); - } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForSize()) {} - fpga->moveStoppingServo(Stop); - } + if (blockSize == HazBlock->size) { + haz = checkColour(colourValues); + } else { + fpga->moveStoppingServo(Go); + while(fpga->checkForSize()) {} + fpga->moveStoppingServo(Stop); + } - 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) + 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); - myLED3 = 0; - // Re-Attach rx interrupt - NVIC_EnableIRQ(UART1_IRQn); + myLED3 = 0; + // Re-Attach rx interrupt + NVIC_EnableIRQ(UART1_IRQn); - return; + 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() { - i2cport = new MCP23017(p9, p10, 0x40); - lcd = new WattBob_TextLCD(i2cport); - myLED1 = 1; - BACKLIGHT_ON(i2cport); - lcd->cls(); - LCDFL(); - lcd->printf("Initilizing..."); - myLED2 = 1; - return; + i2cport = new MCP23017(p9, p10, 0x40); + lcd = new WattBob_TextLCD(i2cport); + myLED1 = 1; + BACKLIGHT_ON(i2cport); + lcd->cls(); + LCDFL(); + lcd->printf("Initilizing..."); + 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[]) { - bool isHazColour[4] = {false, false, false, false }; + bool isHazColour[4] = {false, false, false, false }; - for (int i = 0; i < 4; i++) { - if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { - isHazColour[i] = true; - } - } + for (int i = 0; i < 4; i++) { + if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) { + isHazColour[i] = true; + } + } - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - wait(0.5); - fpga->moveSortingServo(NonHaz); - return true; + fpga->moveSortingServo(Haz); + fpga->moveStoppingServo(Go); + while(fpga->checkForBlock()) {} + fpga->moveStoppingServo(Stop); + wait(0.5); + fpga->moveSortingServo(NonHaz); + return true; - bool isHazBlock = false; + bool isHazBlock = false; - if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]) { - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - fpga->moveSortingServo(NonHaz); - return true; - } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - return false; - } + if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]) { + fpga->moveSortingServo(Haz); + fpga->moveStoppingServo(Go); + while(fpga->checkForBlock()) {} + fpga->moveStoppingServo(Stop); + fpga->moveSortingServo(NonHaz); + return true; + } else { + fpga->moveStoppingServo(Go); + while(fpga->checkForBlock()) {} + fpga->moveStoppingServo(Stop); + return false; + } } 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); + lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition); + lcd->locate(1,0); + lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition); } 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); + printServoInfoOnLCD(); + int finished = false; - 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(0.5); - } else if (button == 2) { - fpga->toggleSortingServo(); - printServoInfoOnLCD(); - wait(0.5); - } - finished = button == 4; - button = 0; - } while (finished == false && runServoTest != false); + 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(0.5); + } else if (button == 2) { + fpga->toggleSortingServo(); + printServoInfoOnLCD(); + wait(0.5); + } + finished = button == 4; + button = 0; + } while (finished == false && runServoTest != false); - 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; + 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; } 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); }