![](/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:
- 15:777390eb5afd
- Parent:
- 14:cf2f255b5560
- Child:
- 17:af373246bf80
--- a/main.cpp Sun Nov 22 17:53:36 2015 +0000 +++ b/main.cpp Sun Nov 22 20:29:08 2015 +0000 @@ -46,13 +46,8 @@ void Rx_interrupt(); void sortBlock(int colourValues[]); void runInServoTestMode(); - -void displayWaitingLine(){ - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Waiting..."); - lcd->locate(1,0); -} +void displayWaitingLine(); +void displayPCStatus(); int main() { @@ -62,7 +57,7 @@ U_LEDS_OFF(); lcd->cls(); myLED1 = 1; - + rgbSensor.enablePowerAndRGBC(); rgbSensor.setIntegrationTime(100); @@ -77,7 +72,7 @@ LCDSL(); i2cport->write_bit(1,13); lcd->printf("2: Connect to PC"); - + fpga->moveStoppingServo(Stop); fpga->moveSortingServo(NonHaz); @@ -99,7 +94,6 @@ bool abortOperation = false; for(;;) { displayWaitingLine(); - lcd->locate(1, 0); lcd->printf("for block."); i2cport->write_bit(1, 15); @@ -124,7 +118,7 @@ D_LEDS_OFF(); break; } - + // Cannot Abort any longer. Block is inserted. // Detach rx interrupt until block processed. NVIC_DisableIRQ(UART1_IRQn); @@ -133,7 +127,7 @@ int blockSize = 0; while (canCheckForSize == 0) { - canCheckForSize = fpga->checkForBlock(); + canCheckForSize = fpga->checkForBlock(); } blockSize = fpga->checkForSize(); @@ -148,13 +142,13 @@ // lcd->locate(0,0); // lcd->printf("Sorting"); sortBlock(colourValues); - }else { - fpga->moveStoppingServo(Go); - while(fpga->checkForSize()){} - fpga->moveStoppingServo(Stop); + } else { + fpga->moveStoppingServo(Go); + while(fpga->checkForSize()) {} + fpga->moveStoppingServo(Stop); } - - myLED3 = 0; + + myLED3 = 0; // Re-Attach rx interrupt NVIC_EnableIRQ(UART1_IRQn); } @@ -163,17 +157,8 @@ if (selection == 2 || connectedToPC == true) { for (;;) { - if (!connectedToPC) { - i2cport->write_bit(1, 13); - // Wait for PC to send '!<pc>connect;' command. - lcd->cls(); - LCDFL(); - lcd->printf("Waiting for PC..."); - wait(0.5); - D_LEDS_OFF(); - LCDSL(); - lcd->printf("4: Main Menu"); - } + displayPCStatus(); + i2cport->write_bit(1, 15); int abortOperation = false; while (connectedToPC == false && abortOperation == false) { @@ -184,18 +169,18 @@ D_LEDS_OFF(); break; } - + + displayPCStatus(); while (abortOperation == false && connectedToPC == true) { - if (runServoTest == true){ - runInServoTestMode(); - lcd->cls(); - lcd->locate(0,0); - lcd->printf("Connected to PC"); - lcd->locate(1,0); - lcd->printf("4:Disconnect"); - i2cport->write_bit(1,15); + if (runServoTest == true) + runInServoTestMode(); + + if (readSwitches() == 4) { + abortOperation = displayAbortDialog(); + if (abortOperation == false) + displayPCStatus(); + } - abortOperation = readSwitches() == 4; } if (abortOperation == true ) { @@ -252,36 +237,37 @@ return; } -void sortBlock(int colourValues[]){ - 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; - } - } - - fpga->moveSortingServo(Haz); - fpga->moveStoppingServo(Go); - while(fpga->checkForBlock()) {} - fpga->moveStoppingServo(Stop); - wait(0.5); - fpga->moveSortingServo(NonHaz); - return; - - 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); - } +void sortBlock(int colourValues[]) +{ + 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; + } + } + + fpga->moveSortingServo(Haz); + fpga->moveStoppingServo(Go); + while(fpga->checkForBlock()) {} + fpga->moveStoppingServo(Stop); + wait(0.5); + fpga->moveSortingServo(NonHaz); + return; + + 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); + } } void printPCDetectedText() @@ -303,7 +289,7 @@ LCDFL(); lcd->printf("Abort?"); LCDSL(); - lcd->printf("1:YES, 2,3,4:NO"); + lcd->printf("1:Yes, 2,3,4:No"); int reply = 0; do { reply = readSwitches(); @@ -311,13 +297,16 @@ 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(){ +void printServoInfoOnLCD() +{ lcd->cls(); lcd->locate(0,0); lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition); @@ -325,28 +314,29 @@ lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition); } -void runInServoTestMode(){ - pc.printf("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", 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; +void runInServoTestMode() +{ + pc.printf("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", 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; + 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(); @@ -357,3 +347,26 @@ lcd->cls(); return; } + +void displayWaitingLine() +{ + 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->locate(1,0); + lcd->printf("4:Disconnect"); + D_LEDS_OFF(); + i2cport->write_bit(1,15); +} \ No newline at end of file