Diff: main.cpp

Revision:

27:2cb1bdb7ae3d

Parent:

26:bbcc25418ffa

Child:

28:7e4d29977d72

--- a/main.cpp	Mon Nov 30 21:29:51 2015 +0000
+++ b/main.cpp	Tue Dec 01 16:16:26 2015 +0000
@@ -30,6 +30,8 @@
 int 		rxIndex = 0;
 float 		percentageError[3];
 float 		adjustedValues[3];
+int 		blockCount = 0;
+bool 		lastBlockHaz = false;
 
 Commander 	_commander = Commander();
 Commander 	*commander = &_commander;
@@ -59,678 +61,723 @@
 
 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(gIntegrationTime);
+    rgbSensor.enablePowerAndRGBC();
+    rgbSensor.setIntegrationTime(gIntegrationTime);
 
-	// 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);
 
-	DefaultHazBlock();
-			fpga->moveStoppingServo(Stop);
-		fpga->moveSortingServo(NonHaz);
-				pc.printf(":<pc>connect;");
-	
-	for (;;) {
-		if (connectedToPC == false){
-		lcd->cls();
-		i2cport->write_bit(1, 12);
-		lcd->printf("1: Start sorting.");
-		LCDSL();
-		i2cport->write_bit(1,13);
-		lcd->printf("2: Connect to PC");
-		}
-		
+    DefaultHazBlock();
+    fpga->moveStoppingServo(Stop);
+    fpga->moveSortingServo(NonHaz);
+    pc.printf(":<pc>connect;");
+    wait(0.2);
+
+    for (;;) {
+        if (connectedToPC == false) {
+            lcd->cls();
+            i2cport->write_bit(1, 12);
+            lcd->locate(0,0);
+            lcd->printf("1: Start sorting.");
+            lcd->locate(1,0);
+            i2cport->write_bit(1,13);
+            lcd->printf("2: Connect to PC");
+        }
+
 //		wait(0.03);
 
-		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);
+//		lcd->cls();
+        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);
+            for(;;) {
+                if (blockCount > 0) {
+                    lcd->cls();
+                    displayWaitingLine();
+                    lcd->locate(1,0);
+                    if (lastBlockHaz == true)
+                        lcd->printf("Hazardous");
+                    else if (lastBlockHaz == false)
+                        lcd->printf("Non-Hazardous");
+                } else {
+                    displayWaitingLine();
+                }
 
-				// Break and return to main menu i.e. Start Op, or Connect to PC.
-				if (waitForBlock() == false) {
-					D_LEDS_OFF();
-					break;
-				} else {
-					sortBlock();
-				}
-			}
-		}
+                i2cport->write_bit(1, 15);
 
-		if (selection == 2 || connectedToPC == true) {
-			for (;;) {
-				displayPCStatus();
+                // Break and return to main menu i.e. Start Op, or Connect to PC.
+                if (waitForBlock() == false) {
+                    D_LEDS_OFF();
+                    break;
+                } else {
+                    sortBlock();
+                }
+            }
+        }
 
-				i2cport->write_bit(1, 15);
-				int abortOperation = false;
-				if (connectedToPC == false)
-					pc.printf(":<pc>connect;");
-				while (connectedToPC == false && abortOperation == false) {
-					abortOperation = readSwitches() == 4;
-				}
+        if (selection == 2 || connectedToPC == true) {
+            wait(0.1);
+            for (;;) {
+                displayPCStatus();
 
-				if (abortOperation == true) {
-					D_LEDS_OFF();
-					break;
-				}
+                i2cport->write_bit(1, 15);
+                int abortOperation = false;
+                if (connectedToPC == false)
+                    pc.printf(":<pc>connect;");
+                while (connectedToPC == false && abortOperation == false) {
+                    abortOperation = readSwitches() == 4;
+                }
 
-				displayPCStatus();
+                if (abortOperation == true) {
+                    D_LEDS_OFF();
+                    break;
+                }
+
+                displayPCStatus();
 
-				while (abortOperation == false && connectedToPC == true) {
-					if (currentMode == Maintanence) {
-						displayPCStatus();
-						while (currentMode == Maintanence){
-						if (runServoTest == true)
-							runInServoTestMode();
-						else if (runBreakBeamTest == true)
-							runInBreakBeamTestMode();
-						else if (runColourSensorTest == true)
-							runInColourSensorTestMode();
-						if (i2cport->read_bit(11) == 1){
-							if (displayAbortDialog() == true){
-								currentMode = None;
-								pc.printf(":<mbed>mode=none;");
-							}else {
-								displayPCStatus();
-							}
-						}
-						}
-					} 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(":<mbed>sort=start;");
-										currentState = Start;
-									} else if (button == 4) {
-										currentMode = None;
-										pc.printf(":<mbed>mode=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 && currentState != Pause) {
-											// TODO: Tell PC to update UI if aborted from MBED.
-											pc.printf(":<mbed>sort=pause;");
-											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) {
+                        displayPCStatus();
+                        while (currentMode == Maintanence) {
+                            if (runServoTest == true)
+                                runInServoTestMode();
+                            else if (runBreakBeamTest == true)
+                                runInBreakBeamTestMode();
+                            else if (runColourSensorTest == true)
+                                runInColourSensorTestMode();
+                            if (i2cport->read_bit(11) == 1) {
+                                if (displayAbortDialog() == true) {
+                                    currentMode = None;
+                                    pc.printf(":<mbed>mode=none;");
+                                } else {
+                                    displayPCStatus();
+                                }
+                            }
+                        }
+                    } 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(":<mbed>sort=start;");
+                                        currentState = Start;
+                                    } else if (button == 4) {
+                                        currentMode = None;
+                                        pc.printf(":<mbed>mode=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 && currentState != Pause) {
+                                            // TODO: Tell PC to update UI if aborted from MBED.
+                                            pc.printf(":<mbed>sort=pause;");
+                                            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;
-	}
-	return false;
+        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
+            return false;
+        else
+            return true;
+    }
+    return false;
 }
 
 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};
+    int numberOfReadings = 3;
+    for (int i = 0; i < 4; 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] / numberOfReadings;
+    }
 
-	bool haz = false;
-	haz = checkColour(colourValues);
+    lastBlockHaz = false;
+    lastBlockHaz = checkColour(averageColourValues);
+
+    if (!lastBlockHaz) {
+        fpga->moveSortingServo(NonHaz);
+    }
 
-	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()) {}
+    myLED3 = 1;
+    int blockSize;
+    while (fpga->checkForBlock() == 0) {  }
+    blockSize = fpga->checkForSize();
+    if (blockSize == HazBlock->size && lastBlockHaz) {
+        //        fpga->moveSortingServo(Haz);
+        //        fpga->moveStoppingServo(Go);
+        //        blockSize = HazBlock->size;
+        while(fpga->getBeamValue(Bottom) == 1) {}
+        wait(kServoWait);
+        fpga->moveStoppingServo(Stop);
+    } else {
+        lastBlockHaz = 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 Pass.\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] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * errorMultiplier))
+                pc.printf("DEBUG:%i Pass.\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], lastBlockHaz, 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], lastBlockHaz);
+    }
 
-	// Re-Attach rx interrupt
-	NVIC_EnableIRQ(UART1_IRQn);
-	myLED3 = 0;
-	myLED4 = 1;
-	return;
+    // Re-Attach rx interrupt
+    NVIC_EnableIRQ(UART1_IRQn);
+
+    blockCount++;
+    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);
+    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));
+    bool isHazColour[3] = {false, false, false };
+    memset(adjustedValues, 0, sizeof(adjustedValues));
+    memset(percentageError, 0, sizeof(percentageError));
+
+    /* Harcoded working
+    	for (int i = 0; i < 3; i++) {
+    		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
+    		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
 
-/* Harcoded working
-	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;
-		}
-	}
-*/
-	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;
+    		}
+    	}
+    */
+    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]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
-			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;
-	//        }
-	//    }
+        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
+            isHazColour[i] = true;
+        }
+    }
 
-	bool isHazBlock = false;
+    //    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;
 
-	if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
-		isHazBlock = true;
-	} else {
-		isHazBlock = false;
-	}
-
-	return isHazBlock;
+    if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
+        isHazBlock = true;
+    } else {
+        isHazBlock = false;
+    }
+    myLED2 = 0;
+    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);
-	if (fpga->stoppingServoPosition == Stop)
-		lcd->printf("1:Top: Go");
-	else if (fpga->stoppingServoPosition == Go)
-		lcd->printf("1:Top: Stop");
+    lcd->cls();
+    lcd->locate(0,0);
+    if (fpga->stoppingServoPosition == Stop)
+        lcd->printf("1:Top: Go");
+    else if (fpga->stoppingServoPosition == Go)
+        lcd->printf("1:Top: Stop");
 
-	lcd->locate(1,0);
-	if (fpga->sortingServoPosition == NonHaz)
-		lcd->printf("2:Bottom: Haz");
-	else if (fpga->sortingServoPosition == Haz)
-		lcd->printf("2:Bottom: NonHaz");
+    lcd->locate(1,0);
+    if (fpga->sortingServoPosition == NonHaz)
+        lcd->printf("2:Bottom: Haz");
+    else if (fpga->sortingServoPosition == Haz)
+        lcd->printf("2:Bottom: NonHaz");
 }
 
-void printServoInfoOnPC() {
-	if (fpga->stoppingServoPosition == Stop)
-		pc.printf(":<servos>1=Stop;");
-	else if (fpga->stoppingServoPosition == Go)
-		pc.printf(":<servos>1=Go;");
+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;");
+    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);
-	printServoInfoOnPC();
-	printServoInfoOnLCD();
+    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();
+
+        // gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
+        if (gToggleServoNumber == 1) {
+            button = 1;
+            gToggleServoNumber = 0;
+        } else if (gToggleServoNumber == 2) {
+            button = 2;
+            gToggleServoNumber = 0;
+        }
 
-	i2cport->write_bit(1, 12);
-	i2cport->write_bit(1, 13);
-	i2cport->write_bit(1, 15);
-	int button = 0;
-	bool finished = false;
-	do {
-		button = readSwitches();
-		
-		// gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
-		if (gToggleServoNumber == 1){
-			button = 1;
-			gToggleServoNumber = 0;
-		}else if (gToggleServoNumber == 2){
-			button = 2;
-			gToggleServoNumber = 0;
-		}
-		
-		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);
+        if (button == 1) {
+            fpga->toggleStoppingServo();
+            printServoInfoOnLCD();
+            printServoInfoOnPC();
+            wait(kServoWait);
+        } else if (button == 2) {
+            fpga->toggleSortingServo();
+            printServoInfoOnLCD();
+            printServoInfoOnPC();
+            wait(kServoWait);
+        }
 
-	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;
+        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;
 }
 
-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 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>2=%i,1=%i;", topBeam, bottomBeam);
+void printBeamInfoOnPC(int topBeam, int bottomBeam)
+{
+    pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
 }
 
-void runInBreakBeamTestMode(){
-	turnOffTopLEDs();
-	i2cport->write_bit(1, 15);
+void runInBreakBeamTestMode()
+{
+    turnOffTopLEDs();
+    i2cport->write_bit(1, 15);
 
-	int topBeamValue = fpga->getBeamValue(1);
-	int bottomBeamValue = fpga->getBeamValue(2);
-	printBeamInfoOnPC(topBeamValue, bottomBeamValue);
-	printBeamInfoOnLCD();
+    int topBeamValue = fpga->getBeamValue(1);
+    int bottomBeamValue = fpga->getBeamValue(2);
+    printBeamInfoOnPC(topBeamValue, bottomBeamValue);
+    printBeamInfoOnLCD();
+
+    int button = 0;
+    bool finished = false;
+    do {
+        button = readSwitches();
+        int currentTopBeamValue = fpga->getBeamValue(1);
+        int currentBottomBeamValue = fpga->getBeamValue(2);
 
-	int button = 0;
-	bool finished = false;
-	do{
-		button = readSwitches();
-		int currentTopBeamValue = fpga->getBeamValue(1);
-		int currentBottomBeamValue = fpga->getBeamValue(2);
-		
-		// For debugging, hold down both 1 and 3 or 2 and 3
-		if (i2cport->read_bit(10) == 1){
-			currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
-			currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
-		}
-		myLED1 = currentTopBeamValue;
-		myLED2 = currentBottomBeamValue;
+        // For debugging, hold down both 1 and 3 or 2 and 3
+        if (i2cport->read_bit(10) == 1) {
+            currentTopBeamValue = i2cport->read_bit(8) && i2cport->read_bit(10);
+            currentBottomBeamValue = i2cport->read_bit(9) && i2cport->read_bit(10);
+        }
+        myLED1 = currentTopBeamValue;
+        myLED2 = currentBottomBeamValue;
 
-		if (currentTopBeamValue != topBeamValue || currentBottomBeamValue != bottomBeamValue ){
-			topBeamValue = currentTopBeamValue;
-			bottomBeamValue = currentBottomBeamValue;
-			printBeamInfoOnPC(topBeamValue, bottomBeamValue);
-		}
-		finished = button == 4;
-	}while (runBreakBeamTest == true && finished == false);
+        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;
+    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 printColourSensorInfoOnLCD(int colourValues[]){
-	float weightedValues[4];
-	
-	for (int i = 0; i < 3; i++){
-		weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
-	}
-	//TODO: Print values on LCD
-	lcd->cls();
+void printColourSensorInfoOnLCD(int colourValues[])
+{
+    float weightedValues[4];
+
+    for (int i = 0; i < 3; i++) {
+        weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
+    }
+    //TODO: Print values on LCD
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("Colour sensor");
+    lcd->locate(1,0);
+    lcd->printf("Test mode");
 }
 
-void printColourSensorInfoOnPC(int colourValues[]){
-	pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
+void printColourSensorInfoOnPC(int colourValues[])
+{
+    pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
 }
 
-void runInColourSensorTestMode() {
-	turnOffTopLEDs();
-	i2cport->write_bit(1, 15);
-	i2cport->write_bit(1, 12);
-	lcd->cls();
-	lcd->locate(0,0);
-	lcd->printf("Colour Sensor");
-	lcd->locate(1,0);
-	lcd->printf("Test Mode");
-	pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
-	
-	int button = 0;
-	bool finished = false;
-	
-	do {
-		button = readSwitches();
-		
-		if (getColourSensorValue == true){
-			int colourValues[4];
-			rgbSensor.getAllColors(colourValues);
-			printColourSensorInfoOnPC(colourValues);
-			printColourSensorInfoOnLCD(colourValues);
-			getColourSensorValue = false;
-		} else if (getBlockColourValue == true){
-			int topBeam = fpga->getBeamValue(1);
-			
-			while(topBeam == 0){
-				topBeam = fpga->getBeamValue(1);
-			}
-			int colourValues[3][4];
-			for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); }
-			int valueCount = 0;
-			do {
-				rgbSensor.getAllColors(colourValues[valueCount]);
-			} while (topBeam == 1 && valueCount < 3);
-			int averageValues[4] = {0, 0, 0, 0};
-			for (int i = 0; i < 3; i++) {
-				averageValues[0] += colourValues[i][0];
-				averageValues[1] += colourValues[i][1];
-				averageValues[2] += colourValues[i][2];
-				averageValues[3] += colourValues[i][3];
-			}
-			averageValues[0] = averageValues[0] / valueCount;
-			averageValues[1] = averageValues[1] / valueCount;
-			averageValues[2] = averageValues[2] / valueCount;
-			averageValues[3] = averageValues[3] / valueCount;
-			
-			printColourSensorInfoOnPC(averageValues);
-			printColourSensorInfoOnLCD(averageValues);
-			
-			getBlockColourValue = false;
-		} else if (button == 1){
-			getColourSensorValue = true;
-			button = 0;
-		}
-		
-		finished = button == 4;
+void runInColourSensorTestMode()
+{
+    turnOffTopLEDs();
+    i2cport->write_bit(1, 15);
+    i2cport->write_bit(1, 12);
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("Colour Sensor");
+    lcd->locate(1,0);
+    lcd->printf("Test Mode");
+    pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
+
+    int button = 0;
+    bool finished = false;
+
+    do {
+        button = readSwitches();
+
+        if (getColourSensorValue == true) {
+            int colourValues[4];
+            rgbSensor.getAllColors(colourValues);
+            printColourSensorInfoOnPC(colourValues);
+            printColourSensorInfoOnLCD(colourValues);
+            getColourSensorValue = false;
+        } else if (getBlockColourValue == true) {
+            lcd->cls();
+            lcd->locate(0,0);
+            lcd->printf("Drop block");
+
+
+            turnOffTopLEDs();
+
+            while(fpga->getBeamValue(Top) == 0) {
+                myLED1 = fpga->getBeamValue(Top);
+                myLED2 = fpga->getBeamValue(Bottom);
+            }
+
+            myLED3 = 1;
+            int colourValues[3][4];
+            for (int i = 0; i < 3; i++) {
+                memset(colourValues[i], 0, sizeof(colourValues));
+            }
+            int valueCount = 0;
+
+            do {
+                rgbSensor.getAllColors(colourValues[valueCount]);
+            } while (fpga->getBeamValue(Top) == 1 && valueCount < 3);
+
+            turnOffTopLEDs();
+            myLED4 = 1;
+
+            int averageValues[4] = {0, 0, 0, 0};
+            for (int i = 0; i < 3; i++) {
+                averageValues[0] += colourValues[i][0];
+                averageValues[1] += colourValues[i][1];
+                averageValues[2] += colourValues[i][2];
+                averageValues[3] += colourValues[i][3];
+            }
+            averageValues[0] = averageValues[0] / valueCount;
+            averageValues[1] = averageValues[1] / valueCount;
+            averageValues[2] = averageValues[2] / valueCount;
+            averageValues[3] = averageValues[3] / valueCount;
+            myLED4 = 0;
+
+            printColourSensorInfoOnPC(averageValues);
+            printColourSensorInfoOnLCD(averageValues);
+
+            getBlockColourValue = false;
+        } else if (button == 1) {
+            getColourSensorValue = true;
+            button = 0;
+        }
+
+        finished = button == 4;
 //		button = 0;
-	} while (finished == false && runColourSensorTest == true);
-	
-	turnOffBottomLEDs();
-	lcd->cls();
-	lcd->locate(0,0);
-	lcd->printf("Finished test");
-	if (runColourSensorTest == true){
-		pc.printf(":<colour_sensor>test=pause;");
-		runColourSensorTest = false;
-	}
-	wait(0.5);
-	return;
+    } while (finished == false && runColourSensorTest == true);
+
+    turnOffBottomLEDs();
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("Finished test");
+    if (runColourSensorTest == true) {
+        pc.printf(":<colour_sensor>test=pause;");
+        runColourSensorTest = 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 == true) {
+        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 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);
+void turnOffBottomLEDs()
+{
+    i2cport->write_bit(0, 12);
+    i2cport->write_bit(0, 13);
+    i2cport->write_bit(0, 14);
+    i2cport->write_bit(0, 15);
 }