![](/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
main.cpp
- Committer:
- dreamselec
- Date:
- 2015-11-24
- Revision:
- 18:44a1c1a30166
- Parent:
- 17:af373246bf80
- Child:
- 19:61b21ac4896e
File content as of revision 18:44a1c1a30166:
#include "mbed.h" #include "WattBob_TextLCD.h" #include "TCS3472_I2C.h" #include "MCP23017.h" #include <string> #include <time.h> //#include <future> #include "globals.h" #include "commander.h" #include "fpga.h" #define BACKLIGHT_ON(INTERFACE) INTERFACE->write_bit(1, 4); #define BACKLIGHT_OFF(INTERFACE) INTERFACE->write_bit(0, 4); #define LCDFL() lcd->locate(0,0); #define LCDSL() lcd->locate(1,0); #define D_LEDS_OFF() i2cport->write_bit(0, 12); i2cport->write_bit(0, 13); i2cport->write_bit(0, 14); i2cport->write_bit(0, 15); #define U_LEDS_OFF() myLED1 = 0; myLED2 = 0; myLED3 = 0; myLED4 = 0; DigitalOut myLED1(LED1); DigitalOut myLED2(LED2); DigitalOut myLED3(LED3); DigitalOut myLED4(LED4); MCP23017 *i2cport; WattBob_TextLCD *lcd; TCS3472_I2C rgbSensor(p28, p27); Serial pc(USBTX, USBRX); uint8_t rxBuffer[kSmallBufferSize + 1]; int rxIndex = 0; Commander _commander = Commander(); Commander *commander = &_commander; FPGA _fpga = FPGA(); extern FPGA *fpga = &_fpga; extern Block _HazBlock; Block *HazBlock = &_HazBlock; extern PCModes currentMode; void initInternal(); void initPort(int baudRate=kDefaultBaudRate); void printPCDetectedText(); bool displayAbortDialog(); void Rx_interrupt(); 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; 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); 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(); for(;;) { displayWaitingLine(); lcd->printf("for block."); i2cport->write_bit(1, 15); waitForBlock(&abortOpration); // 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(); i2cport->write_bit(1, 15); int abortOperation = false; while (connectedToPC == false && abortOperation == false) { abortOperation = readSwitches() == 4; } if (abortOperation == true) { D_LEDS_OFF(); break; } 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(); // } } 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); if (abortOperation == true || connectedToPC == true) 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); int canCheckForSize = fpga->checkForBlock(); int blockSize = 0; while (canCheckForSize == 0) { canCheckForSize = fpga->checkForBlock(); } 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 (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); 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); } 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; } void initPort(int baudRate) { 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 }; 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; 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; } } void printPCDetectedText() { 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) { // 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); } 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; 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); } 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->locate(1,0); lcd->printf("4:Disconnect"); D_LEDS_OFF(); i2cport->write_bit(1,15); }