Diff: main.cpp

Revision:

19:61b21ac4896e

Parent:

18:44a1c1a30166

Child:

20:4e0f0944f28f

--- a/main.cpp	Tue Nov 24 00:24:39 2015 +0000
+++ b/main.cpp	Tue Nov 24 01:38:46 2015 +0000
@@ -54,383 +54,411 @@
 
 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(2.4);
+    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();
+
+            for(;;) {
+                displayWaitingLine();
+                lcd->printf("for block.");
 
-			for(;;) {
-				displayWaitingLine();
-				lcd->printf("for block.");
-
-				i2cport->write_bit(1, 15);
-				waitForBlock(&abortOpration);
+                i2cport->write_bit(1, 15);
 
-				// 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 (waitForBlock() == false) {
+                    D_LEDS_OFF();
+                    break;
+                } else {
+                    sortBlock();
+                }
+            }
+        }
 
-		if (selection == 2 || connectedToPC == true) {
-			for (;;) {
-				displayPCStatus();
+        if (selection == 2 || connectedToPC == true) {
+            for (;;) {
+                displayPCStatus();
+
+                i2cport->write_bit(1, 15);
+                int abortOperation = false;
+                while (connectedToPC == false && abortOperation == false) {
+                    abortOperation = readSwitches() == 4;
+                }
 
-				i2cport->write_bit(1, 15);
-				int abortOperation = false;
-				while (connectedToPC == false && abortOperation == false) {
-					abortOperation = readSwitches() == 4;
-				}
+                if (abortOperation == true) {
+                    D_LEDS_OFF();
+                    break;
+                }
 
-				if (abortOperation == true) {
-					D_LEDS_OFF();
-					break;
-				}
-
-				displayPCStatus();
+                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();
-//					}
-				}
+                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) {
+                                        currentState = Pause;
+                                        if (connectedToPC == true) {
+                                            // TODO: Tell PC to update UI if aborted from MBED.
+                                            pc.printf("state:pause\n");
 
-				if (abortOperation == true ) {
-					connectedToPC = false;
-					D_LEDS_OFF();
-					break;
-				}
-			}
-		}
+                                            continue;
+                                        } else if (connectedToPC == false) {
+                                            currentMode = None;
+                                            abortOperation = true;
+                                        }
+                                    } else {
+                                        sortBlock();
+                                    }
+                                }
+                            }
+                        }
+                    } else if (currentMode == None) {
+setModeNone:
+                        displayPCStatus();
+                        while (currentMode == None && abortOperation == false) {
+                            abortOperation = readSwitches() == 4;
+                        }
+                    }
+                }
 
-	}
+                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);
+bool waitForBlock()
+{
+    if (connectedToPC == false) {
+        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 || connectedToPC == true)
+            return false;
+        else
+            return true;
+    } else if (connectedToPC == true) {
+        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 == true);
+
+        if (abortOperation == true || connectedToPC == false)
+            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);
+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;
+    int canCheckForSize = fpga->checkForBlock();
+    int blockSize = 0;
 
-	while (canCheckForSize == 0) {
-		canCheckForSize = fpga->checkForBlock();
-	}
+    while (canCheckForSize == 0) {
+        canCheckForSize = fpga->checkForBlock();
+    }
 
-	blockSize = fpga->checkForSize();
-	myLED3 = blockSize;
-	bool haz = false;
+    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 (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);
+    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);
+    myLED3 = 0;
+    // Re-Attach rx interrupt
+    NVIC_EnableIRQ(UART1_IRQn);
 
-	return;
+    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;
 }
 
 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 true;
+    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);
-		return true;
-	} else {
-		fpga->moveStoppingServo(Go);
-		while(fpga->checkForBlock()) {}
-		fpga->moveStoppingServo(Stop);
-		return 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();
+    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);
+    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;
+    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) {
-		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->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);
+    lcd->locate(1,0);
+    lcd->printf("4:Disconnect");
+    D_LEDS_OFF();
+    i2cport->write_bit(1,15);
 }