![](/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:
- 10:16ba52f8e025
- Parent:
- 9:dc8f155b71c8
- Child:
- 11:0fe833f8a1ab
--- a/main.cpp Mon Nov 16 23:44:44 2015 +0000 +++ b/main.cpp Wed Nov 18 16:09:52 2015 +0000 @@ -25,7 +25,7 @@ MCP23017 *i2cport; WattBob_TextLCD *lcd; -TCS3472_I2C rgb_sensor(p28, p27); +TCS3472_I2C rgbSensor(p28, p27); Serial pc(USBTX, USBRX); uint8_t rxBuffer[kSmallBufferSize + 1]; int rxIndex = 0; @@ -41,95 +41,122 @@ void detectPC(); void initInternal(); void initPort(int baudRate=kDefaultBaudRate); -int readSwitches(); void printPCDetectedText(); -bool displayAboardDialog(); +bool displayAbortDialog(); void Rx_interrupt(); +void sortBlock(int colourValues[]); +void runInServoTestMode(); +void displayWaitingLine(){ + lcd->cls(); + lcd->locate(0,0); + lcd->printf("Waiting..."); + lcd->locate(1,0); +} 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); - // 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"); + // 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); - 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(;;) { - lcd->cls(); - lcd->printf("Waiting..."); - - i2cport->write_bit(1, 15); - bool aboardOperation = false; + 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); + + 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(); + + i2cport->write_bit(1, 15); + - int blockInserted = 0; - // Wait until a block is breaking the beam, or button 4 is pressed to aboard. - do { - blockInserted = fpga->checkForBlock(); - if (i2cport->read_bit(11)) { - aboardOperation = displayAboardDialog(); - // Cancel the aboard - if (aboardOperation == false) { - lcd->cls(); - LCDFL(); - lcd->printf("Waiting..."); - i2cport->write_bit(1, 15); - } - } - } while (aboardOperation == false && blockInserted != 1); + int blockInserted = 0; + // Wait until a block is breaking the beam, or button 4 is pressed to abort. + do { + blockInserted = fpga->checkForBlock(); + 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 (abortOperation == true || connectedToPC == true) { + D_LEDS_OFF(); + break; + } + + // Cannot Abort any longer. Block is inserted. + // Detach rx interrupt until block processed. + NVIC_DisableIRQ(UART1_IRQn); - // Break and return to main menu i.e. Start Op, or Connect to PC. - if (aboardOperation == true) { - D_LEDS_OFF(); - break; - } - - // Cannot aboard any longer. Block is inserted. - // Detach rx interrupt until block processed. - NVIC_DisableIRQ(UART1_IRQn); - - int blockSize = fpga->checkForSize(); + int blockSize = 0; + blockSize = fpga->checkForSize(); + myLED3 = blockSize; - if (blockSize == HazBlock->size) { - // detectColour() - lcd->cls(); - lcd->printf("Detecting Colour"); - } + if (blockSize == 0) { + // 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); + } + + myLED3 = 0; + // Re-Attach rx interrupt + NVIC_EnableIRQ(UART1_IRQn); + } +// if (abortOperation == true || connectedToPC == true){ break; } + } - // Re-Attach rx interrupt - NVIC_EnableIRQ(UART1_IRQn); - } - - } else if (selection == 2 || connectedToPC == true) { - for (;;){ + if (selection == 2 || connectedToPC == true) { + for (;;) { if (!connectedToPC) { i2cport->write_bit(1, 13); // Wait for PC to send '!<pc>connect;' command. @@ -141,119 +168,191 @@ LCDSL(); lcd->printf("4: Main Menu"); } - i2cport->write_bit(1, 15); - int aboardOperation = false; - while (connectedToPC == false && aboardOperation == false){ - aboardOperation = readSwitches() == 4; - } - if (aboardOperation == true) { D_LEDS_OFF(); break; } - else if (connectedToPC == true){ - lcd->cls(); - LCDFL(); - lcd->printf("Connected to PC"); - LCDSL(); - lcd->printf("4: Disconnect."); - } + i2cport->write_bit(1, 15); + int abortOperation = false; + while (connectedToPC == false && abortOperation == false) { + abortOperation = readSwitches() == 4; + } + if (abortOperation == true) { + D_LEDS_OFF(); + break; + } else if (connectedToPC == true) { + lcd->cls(); + LCDFL(); + lcd->printf("Connected to PC"); + LCDSL(); + lcd->printf("4: Disconnect."); + } - while (aboardOperation == false && connectedToPC == true){ - aboardOperation = readSwitches() == 4; - } - - if (aboardOperation == true ) { D_LEDS_OFF(); break; } - } - } + 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); + } + abortOperation = readSwitches() == 4; + } - } + if (abortOperation == true ) { + connectedToPC = false; + D_LEDS_OFF(); + break; + } + } + } + + } } /// 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); +void Rx_interrupt() +{ + 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; } -int readSwitches() -{ - - if(i2cport->read_bit(8)) { - return 1; - } else if (i2cport->read_bit(9)) { - return 2; - } else if (i2cport->read_bit(10)) { - return 3; - } else if (i2cport->read_bit(11)) { - return 4; - } else { - return 0; +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() { - 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); + + 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) { + return true; + } else { + return false; + } } -bool displayAboardDialog() -{ - while (i2cport->read_bit(11) == 1){} - i2cport->write_bit(1, 12); +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); +} + +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; - lcd->cls(); - LCDFL(); - lcd->printf("Aboard?"); - LCDSL(); - lcd->printf("1:YES, 2,3,4:NO"); - int reply = 0; - do { - reply = readSwitches(); - } while(reply == 0); - - D_LEDS_OFF(); - if (reply == 1) { - return true; - } else { - return 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; }