![](/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:
- 18:44a1c1a30166
- Parent:
- 17:af373246bf80
- Child:
- 19:61b21ac4896e
--- a/main.cpp Mon Nov 23 23:13:39 2015 +0000 +++ b/main.cpp Tue Nov 24 00:24:39 2015 +0000 @@ -38,335 +38,399 @@ extern Block _HazBlock; Block *HazBlock = &_HazBlock; -void detectPC(); +extern PCModes currentMode; + void initInternal(); void initPort(int baudRate=kDefaultBaudRate); void printPCDetectedText(); bool displayAbortDialog(); void Rx_interrupt(); -void sortBlock(int colourValues[]); +bool checkColour(int colourValues[]); void runInServoTestMode(); void displayWaitingLine(); void displayPCStatus(); +bool waitForBlock(); +void sortBlock(); 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(100); + 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(); - bool abortOperation = false; - for(;;) { - displayWaitingLine(); - lcd->printf("for block."); + for(;;) { + displayWaitingLine(); + lcd->printf("for block."); + + i2cport->write_bit(1, 15); + waitForBlock(&abortOpration); - i2cport->write_bit(1, 15); - 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); + // 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 (abortOperation == true || connectedToPC == true) { - D_LEDS_OFF(); - break; - } + if (selection == 2 || connectedToPC == true) { + for (;;) { + displayPCStatus(); + + i2cport->write_bit(1, 15); + int abortOperation = false; + while (connectedToPC == false && abortOperation == false) { + abortOperation = readSwitches() == 4; + } + + if (abortOperation == true) { + D_LEDS_OFF(); + break; + } + + displayPCStatus(); - // Cannot Abort any longer. Block is inserted. - // Detach rx interrupt until block processed. - NVIC_DisableIRQ(UART1_IRQn); - - int canCheckForSize = fpga->checkForBlock(); - int blockSize = 0; - - while (canCheckForSize == 0) { - canCheckForSize = fpga->checkForBlock(); - } - - blockSize = fpga->checkForSize(); - myLED3 = blockSize; + 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(); +// } + } - if (blockSize == Block::Small) { - // detectColour() - int colourValues[4]; -// displayWaitingLine(); - rgbSensor.getAllColors(colourValues); -// lcd->cls(); -// lcd->locate(0,0); -// lcd->printf("Sorting"); - sortBlock(colourValues); - } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForSize()) {} - fpga->moveStoppingServo(Stop); - } + if (abortOperation == true ) { + connectedToPC = false; + D_LEDS_OFF(); + break; + } + } + } - myLED3 = 0; - // Re-Attach rx interrupt - NVIC_EnableIRQ(UART1_IRQn); - } -// if (abortOperation == true || connectedToPC == true){ break; } - } + } +} - if (selection == 2 || connectedToPC == true) { - for (;;) { - displayPCStatus(); - - i2cport->write_bit(1, 15); - int abortOperation = false; - while (connectedToPC == false && abortOperation == false) { - abortOperation = readSwitches() == 4; - } +// 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); + + if (abortOperation == true || connectedToPC == true) + return false; + else + return true; +} - if (abortOperation == true) { - D_LEDS_OFF(); - break; - } - - displayPCStatus(); - while (abortOperation == false && connectedToPC == true) { - if (runServoTest == true) - runInServoTestMode(); +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; + + while (canCheckForSize == 0) { + canCheckForSize = fpga->checkForBlock(); + } - if (readSwitches() == 4) { - abortOperation = displayAbortDialog(); - if (abortOperation == false) - displayPCStatus(); - - } - } + blockSize = fpga->checkForSize(); + myLED3 = blockSize; + bool haz = false; - if (abortOperation == true ) { - connectedToPC = false; - D_LEDS_OFF(); - break; - } - } - } + 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); + + myLED3 = 0; + // Re-Attach rx interrupt + NVIC_EnableIRQ(UART1_IRQn); + + 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; } -void sortBlock(int colourValues[]) +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; + 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); - } else { - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - } + 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); - - D_LEDS_OFF(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Done servo test"); - 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(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; } 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) { - 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); -} \ No newline at end of file + lcd->locate(1,0); + lcd->printf("4:Disconnect"); + D_LEDS_OFF(); + i2cport->write_bit(1,15); +}