Diff: main.cpp

Revision:

23:db91aaa43a9e

Parent:

22:993821a4c396

Child:

24:02c61793f90b

--- a/main.cpp	Thu Nov 26 15:47:17 2015 +0000
+++ b/main.cpp	Sat Nov 28 23:28:22 2015 +0000
@@ -45,7 +45,6 @@
 void initInternal();
 void initPort(int baudRate=kDefaultBaudRate);
 void printPCDetectedText();
-//bool displayAbortDialog();
 void Rx_interrupt();
 bool checkColour(int colourValues[]);
 void runInServoTestMode();
@@ -53,454 +52,551 @@
 void displayPCStatus();
 bool waitForBlock();
 void sortBlock();
+void runInBreakBeamTestMode();
+void turnOffTopLEDs();
+void turnOffBottomLEDs();
 
 int main()
 {
-    initInternal();
-    initPort();
-    srand((unsigned)time(NULL));
-    U_LEDS_OFF();
-    lcd->cls();
+	initInternal();
+	initPort();
+	srand((unsigned)time(NULL));
+	U_LEDS_OFF();
+	lcd->cls();
 
-    rgbSensor.enablePowerAndRGBC();
-    rgbSensor.setIntegrationTime(3);
+	rgbSensor.enablePowerAndRGBC();
+	rgbSensor.setIntegrationTime(3);
 
-    // 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);
+				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;
+				if (connectedToPC == false)
+					pc.printf(":<pc>connect;");
+				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;
-                                i2cport->write_bit(1, 12);
-                                i2cport->write_bit(1, 15);
-                                while (currentState == Pause && currentMode == Normal) {
-                                    button = readSwitches();
-                                    if (button == 1) {
-                                        pc.printf("state:start\n");
-                                        currentState = Start;
-                                    } else if (button == 4) {
-                                        currentMode = None;
-//                                        goto setModeNone;
-                                        break;
-                                    }
-                                }
-                            }
-                            if (currentState == Start) {
-                                lcd->cls();
-                                lcd->locate(0,0);
-                                lcd->printf("Sorting mode...");
-                                while (currentState == Start && currentMode == Normal) {
-                                    if (waitForBlock() == false) {
-                                        if (connectedToPC == true) {
-                                            // TODO: Tell PC to update UI if aborted from MBED.
-                                            pc.printf("state:pause\n");
-                                            currentState = Pause;
-                                            continue;
-                                        } else if (connectedToPC == false) {
-                                            currentMode = None;
-                                            abortOperation = true;
-                                        }
-                                    } else {
-                                        sortBlock();
-                                    }
-                                }
-                            }
-                        }
-                    } else if (currentMode == None) {
-setModeNone:
-                        D_LEDS_OFF();
-                        i2cport->write_bit(1,15);
-                        displayPCStatus();
-                        while (currentMode == None && abortOperation == false && connectedToPC == true) {
-                            if (i2cport->read_bit(11)) {
-                                abortOperation = displayAbortDialog();
-                                // Cancel the Abort
-                                if (abortOperation == false) {
-                                    displayPCStatus();
-                                    i2cport->write_bit(1, 15);
-                                }
-                            }
-                        }
-                    }
-                }
+				while (abortOperation == false && connectedToPC == true) {
+					if (currentMode == Maintanence) {
+						if (runServoTest == true)
+							runInServoTestMode();
+						else if (runBreakBeamTest == true)
+							runInBreakBeamTestMode();
+					} 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;
+								i2cport->write_bit(1, 12);
+								i2cport->write_bit(1, 15);
+								while (currentState == Pause && currentMode == Normal) {
+									button = readSwitches();
+									if (button == 1) {
+										pc.printf("state:start\n");
+										currentState = Start;
+									} else if (button == 4) {
+										currentMode = None;
+										//                                        goto setModeNone;
+										break;
+									}
+								}
+							}
+							if (currentState == Start) {
+								lcd->cls();
+								lcd->locate(0,0);
+								lcd->printf("Sorting mode...");
+								while (currentState == Start && currentMode == Normal) {
+									if (waitForBlock() == false) {
+										if (connectedToPC == true) {
+											// TODO: Tell PC to update UI if aborted from MBED.
+											pc.printf("state:pause\n");
+											currentState = Pause;
+											continue;
+										} else if (connectedToPC == false) {
+											currentMode = None;
+											abortOperation = true;
+										}
+									} else {
+										sortBlock();
+									}
+								}
+							}
+						}
+					} else if (currentMode == None) {
+						setModeNone:
+						D_LEDS_OFF();
+						i2cport->write_bit(1,15);
+						displayPCStatus();
+						while (currentMode == None && abortOperation == false && connectedToPC == true) {
+							if (i2cport->read_bit(11)) {
+								abortOperation = displayAbortDialog();
+								// Cancel the Abort
+								if (abortOperation == false) {
+									displayPCStatus();
+									i2cport->write_bit(1, 15);
+								}
+							}
+						}
+					}
+				}
 
-                if (abortOperation == true ) {
-                    connectedToPC = false;
-                    D_LEDS_OFF();
-                    break;
-                }
-            }
-        }
+				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()
 {
-    myLED4 = 0;
-    myLED1 = 1;
-    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);
+	myLED4 = 0;
+	myLED1 = 1;
+	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;
-    } 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 && currentState == Start && currentMode == Normal);
+		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 && currentState == Start && currentMode == Normal);
 
-        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
-            return false;
-        else
-            return true;
-    }
+		if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
+			return false;
+		else
+			return true;
+	}
 }
 
 void sortBlock()
 {
-    myLED1 = 0;
-    myLED2 = 1;
-    // Cannot Abort any longer. Block is inserted.
-    // Detach rx interrupt until block processed.
-    NVIC_DisableIRQ(UART1_IRQn);
-    fpga->moveSortingServo(Haz);
-    fpga->moveStoppingServo(Go);
+	myLED1 = 0;
+	myLED2 = 1;
+	// Cannot Abort any longer. Block is inserted.
+	// Detach rx interrupt until block processed.
+	NVIC_DisableIRQ(UART1_IRQn);
+	fpga->moveSortingServo(Haz);
+	fpga->moveStoppingServo(Go);
 
-    int colourValues[4];
-    int averageColourValues[4] = {0, 0, 0, 0};
-    for (int i = 0; i < 3; i++){
-    	rgbSensor.getAllColors(colourValues);
-    	for (int j = 0; j < 4; j++){
-    		averageColourValues[j] += colourValues[j];
-    	}
-    }
-    for (int i = 0; i < 4; i++){
-    	averageColourValues[i] = averageColourValues[i] / 3;
-    }
+	int colourValues[4];
+	int averageColourValues[4] = {0, 0, 0, 0};
+	for (int i = 0; i < 3; i++){
+		rgbSensor.getAllColors(colourValues);
+		for (int j = 0; j < 4; j++){
+			averageColourValues[j] += colourValues[j];
+		}
+	}
+	for (int i = 0; i < 4; i++){
+		averageColourValues[i] = averageColourValues[i] / 3;
+	}
 
-    bool haz = false;
-    haz = checkColour(colourValues);
-    
-    if (!haz) {
-        fpga->moveSortingServo(NonHaz);
-    }
+	bool haz = false;
+	haz = checkColour(colourValues);
+
+	if (!haz) {
+		fpga->moveSortingServo(NonHaz);
+	}
 	int blockSize;
-    while (fpga->checkForBlock() == 0) {  }
-    blockSize = fpga->checkForSize();
-    if (blockSize == HazBlock->size && haz) {
-//        fpga->moveSortingServo(Haz);
-//        fpga->moveStoppingServo(Go);
-//        blockSize = HazBlock->size;
-        while(fpga->getBeamValue(Bottom) == 1) {}
-        wait(kServoWait);
-        fpga->moveStoppingServo(Stop);
-    } else {
-        haz = false;
-    }
-    fpga->moveSortingServo(NonHaz);
-    while(fpga->checkForSize()) {}
+	while (fpga->checkForBlock() == 0) {  }
+	blockSize = fpga->checkForSize();
+	if (blockSize == HazBlock->size && haz) {
+		//        fpga->moveSortingServo(Haz);
+		//        fpga->moveStoppingServo(Go);
+		//        blockSize = HazBlock->size;
+		while(fpga->getBeamValue(Bottom) == 1) {}
+		wait(kServoWait);
+		fpga->moveStoppingServo(Stop);
+	} else {
+		haz = false;
+	}
+	fpga->moveSortingServo(NonHaz);
+	while(fpga->checkForSize()) {}
 
-    if (connectedToPC) {
-        for (int i = 0; i < 3; i++) {
-            pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
-            if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
-            	pc.printf("DEBUG:%i.\n", i);
-        }
-        pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
-//        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
-    }
+	if (connectedToPC) {
+		for (int i = 0; i < 3; i++) {
+			pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
+			if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
+				pc.printf("DEBUG:%i.\n", i);
+		}
+		pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
+		//        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
+	}
 
-    // Re-Attach rx interrupt
-    NVIC_EnableIRQ(UART1_IRQn);
-    myLED3 = 0;
-    myLED4 = 1;
-    return;
+	// Re-Attach rx interrupt
+	NVIC_EnableIRQ(UART1_IRQn);
+	myLED3 = 0;
+	myLED4 = 1;
+	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()
 {
-    myLED1 = 1;
-    i2cport = new MCP23017(p9, p10, 0x40);
-    lcd = new WattBob_TextLCD(i2cport);
-    BACKLIGHT_ON(i2cport);
-    lcd->cls();
-    lcd->locate(0,0);
-    lcd->printf("Initilizing...");
-    DefaultHazBlock();
-    myLED2 = 1;
-    return;
+	myLED1 = 1;
+	i2cport = new MCP23017(p9, p10, 0x40);
+	lcd = new WattBob_TextLCD(i2cport);
+	BACKLIGHT_ON(i2cport);
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Initilizing...");
+	DefaultHazBlock();
+	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);
+	pc.printf(":<pc>connect;");
+	return;
 }
 
 bool checkColour(int colourValues[])
 {
-    myLED2 = 0;
-    myLED3 = 1;
-    bool isHazColour[3] = {false, false, false };
-    memset(adjustedValues, 0, sizeof(adjustedValues));
-    memset(percentageError, 0, sizeof(percentageError));
+	myLED2 = 0;
+	myLED3 = 1;
+	bool isHazColour[3] = {false, false, false };
+	memset(adjustedValues, 0, sizeof(adjustedValues));
+	memset(percentageError, 0, sizeof(percentageError));
 
-    for (int i = 0; i < 3; i++) {
-        adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
-        percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
+	for (int i = 0; i < 3; i++) {
+		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
+		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
 
-        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
-            isHazColour[i] = true;
-        }
-    }
+		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
+			isHazColour[i] = true;
+		}
+	}
 
-//    for (int i = 0; i < 3; i++) {
-//        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
-//            isHazColour[i] = true;
-//        }
-//    }
+	//    for (int i = 0; i < 3; i++) {
+		//        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
+	//            isHazColour[i] = true;
+	//        }
+	//    }
 
-    bool isHazBlock = false;
+	bool isHazBlock = false;
 
-    if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
-        isHazBlock = true;
-    } else {
-        isHazBlock = false;
-    }
-//    pc.printf("DEBUG:haz: %i\n", isHazBlock);
-    return isHazBlock;
+	if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
+		isHazBlock = true;
+	} else {
+		isHazBlock = false;
+	}
+
+	return isHazBlock;
 }
 
 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);
+	if (fpga->stoppingServoPosition == Stop)
+		lcd->printf("1: Stopping servo: Stop");
+	else if (fpga->stoppingServoPosition == Go)
+		lcd->printf("1: Stopping servo: Go");
+
+	lcd->locate(1,0);
+	if (fpga->sortingServoPosition == NonHaz)
+		lcd->printf("2: Sorting servo: NonHaz");
+	else if (fpga->sortingServoPosition == Haz)
+		lcd->printf("2: Sorting serov: Haz");
+}
+
+void printServoInfoOnPC() {
+	if (fpga->stoppingServoPosition == Stop)
+		pc.printf(":<servos>1=Stop;");
+	else if (fpga->stoppingServoPosition == Go)
+		pc.printf(":<servos>1=Go;");
+
+	if (fpga->sortingServoPosition == NonHaz)
+		pc.printf(":<servos>2=NonHaz;");
+	else if (fpga->sortingServoPosition == Haz)
+		pc.printf(":<servos>2=Haz;");
 }
 
 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);
+	printServoInfoOnPC();
+	printServoInfoOnLCD();
+
+	i2cport->write_bit(1, 12);
+	i2cport->write_bit(1, 13);
+	i2cport->write_bit(1, 15);
+	int button = 0;
+	bool finished = false;
+	do {
+		button = readSwitches();
+		if (button == 1) {
+			fpga->toggleStoppingServo();
+			printServoInfoOnLCD();
+			printServoInfoOnPC();
+			wait(kServoWait);
+		} else if (button == 2) {
+			fpga->toggleSortingServo();
+			printServoInfoOnLCD();
+			printServoInfoOnPC();
+			wait(kServoWait);
+		}
+		finished = button == 4;
+		button = 0;
+	} while (finished == false && runServoTest == true);
+
+	D_LEDS_OFF();
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Done servo test");
+	if (runServoTest == true){
+		pc.printf(":<servos>test=pause;");
+		runServoTest = false;
+	}
+	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(kServoWait);
-        } else if (button == 2) {
-            fpga->toggleSortingServo();
-            printServoInfoOnLCD();
-            wait(kServoWait);
-        }
-        finished = button == 4;
-        button = 0;
-    } while (finished == false && runServoTest != false);
+void printBeamInfoOnLCD(){
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Top:L1 Btm: L2");
+	lcd->locate(1,0);
+	lcd->printf("On:High, Off:Low");
+}
+
+void printBeamInfoOnPC(int topBeam, int bottomBeam){
+	pc.printf(":<break_beam>1=%i,2=%i;", topBeam, bottomBeam);
+}
+
+void runInBreakBeamTestMode(){
+	turnOffTopLEDs();
+	i2cport->write_bit(1, 15);
+
+	int topBeamValue = fpga->getBeamValue(1);
+	int bottomBeamValue = fpga->getBeamValue(2);
+	printBeamInfoOnPC(topBeamValue, bottomBeamValue);
+	printBeamInfoOnLCD();
 
-    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;
+	int button = 0;
+	bool finished = false;
+	do{
+		button = readSwitches();
+		int currentTopBeamValue = fpga->getBeamValue(1);
+		int currentBottomBeamValue = fpga->getBeamValue(2);
+		myLED1 = currentTopBeamValue;
+		myLED2 = currentBottomBeamValue;
+
+		if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ){
+			topBeamValue = currentTopBeamValue;
+			bottomBeamValue = currentBottomBeamValue;
+			printBeamInfoOnPC(topBeamValue, bottomBeamValue);
+		}
+		finished = button == 4;
+	}while (runBreakBeamTest == true && finished == false);
+
+	turnOffBottomLEDs();
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Finished test");
+	if (runBreakBeamTest == true){
+		pc.printf(":<break_beam>test=pause;");
+		runBreakBeamTest = false;
+	}
+	wait(0.5);
+	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);
 }
+
+void turnOffTopLEDs(){
+	myLED1 = 0;
+	myLED2 = 0;
+	myLED3 = 0;
+	myLED4 = 0;
+}
+
+void turnOffBottomLEDs(){
+	i2cport->write_bit(0, 12);
+	i2cport->write_bit(0, 13);
+	i2cport->write_bit(0, 14);
+	i2cport->write_bit(0, 15);
+}