Diff: main.cpp

Revision:

18:44a1c1a30166

Parent:

17:af373246bf80

Child:

19:61b21ac4896e

diff -r af373246bf80 -r 44a1c1a30166 main.cpp
--- 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);
+}