Diff: globals.cpp

Revision:

20:4e0f0944f28f

Parent:

19:61b21ac4896e

Child:

22:993821a4c396

--- a/globals.cpp	Tue Nov 24 01:38:46 2015 +0000
+++ b/globals.cpp	Tue Nov 24 20:53:51 2015 +0000
@@ -6,150 +6,188 @@
 #include "WattBob_TextLCD.h"
 
 int kDefaultBaudRate = 19200;
-int ColourSensorError = 25;
+//TODO: Not let it be constant.
+int ColourSensorError = 0.5;
 //SerialBase gParity = SerialBase::None;
 int gStopBits = 1;
 
-Block _HazBlock = Block(kDefaultHazBlock);
+
+Block _HazBlock = Block();
 
 bool connectedToPC = false;
 bool runServoTest = false;
 PCModes currentMode = None;
 Controls currentState = Pause;
 
+void DefaultHazBlock(){
+	Colour _minRedBlock = Colour();
+	Colour _maxRedBlock = Colour();
+	Colour _averageRedBlock = Colour();
+	
+	for (int i = 0; i < 4; i++){
+		_minRedBlock.components[i] = kMinRedBlock[i];
+		_maxRedBlock.components[i] = kMaxRedBlock[i];
+		_averageRedBlock.components[i] = kAverageRedBlock[i];
+	}
+	
+	_HazBlock.minColour = Colour(_minRedBlock);
+	_HazBlock.maxColour = Colour(_maxRedBlock);
+	_HazBlock.averageColour = Colour(_averageRedBlock);
+	_HazBlock.size = Block::Small;
+	
+//	defaultHazBlock.minColour = Colour(_minRedBlock);
+//	defaultHazBlock.maxColour = Colour(_maxRedBlock);
+//	defaultHazBlock.averageColour = Colour(_averageRedBlock);
+	
+//	Block _HazBlock = Block(defaultHazBlock);
+}
+
 int readSwitches()
 {
-	if(i2cport->read_bit(8)) {
-		while (i2cport->read_bit(8)) { }
-		return 1;
-	} else if (i2cport->read_bit(9)) {
-		while (i2cport->read_bit(9)) { }
-		return 2;
-	} else if (i2cport->read_bit(10)) {
-		while (i2cport->read_bit(10)) { }
-		return 3;
-	} else if (i2cport->read_bit(11)) {
-		while (i2cport->read_bit(11)) { }
-		return 4;
-	} else {
-		return 0;
-	}
+    if(i2cport->read_bit(8)) {
+        while (i2cport->read_bit(8)) { }
+        return 1;
+    } else if (i2cport->read_bit(9)) {
+        while (i2cport->read_bit(9)) { }
+        return 2;
+    } else if (i2cport->read_bit(10)) {
+        while (i2cport->read_bit(10)) { }
+        return 3;
+    } else if (i2cport->read_bit(11)) {
+        while (i2cport->read_bit(11)) { }
+        return 4;
+    } else {
+        return 0;
+    }
 
 }
 
 void connectToPC(CommandTypeRaw typeRaw)
 {
-	connectedToPC = true;
-	pc.printf("INFO:MBED connected to PC.\n");
+    connectedToPC = true;
+    pc.printf("INFO:MBED connected to PC.\n");
 }
 
 void disconnectToPC(CommandTypeRaw typeRaw)
 {
-	pc.printf("INFO:MBED disconnected from PC.\n");
-	connectedToPC = false;
-	currentMode = None;
+    pc.printf("INFO:MBED disconnected from PC.\n");
+    connectedToPC = false;
+    currentMode = None;
 }
 
 void hazBlock(CommandTypeRaw typeRaw)
 {
-	if (typeRaw == Set) {
-		pc.printf("INFO:Setting new haz block.\n");
-		int lowerBeam = 0;
-		int higherBeam = 0;
-		char readyChar = '\0';
+    if (typeRaw == Set) {
+        pc.printf("INFO:Setting new haz block.\n");
+        int lowerBeam = 0;
+        int higherBeam = 0;
+        char readyChar = '\0';
+
+        lcd->cls();
+        lcd->locate(0,0);
+        lcd->printf("New haz block");
 
-		do { higherBeam = fpga->checkForSize();
-		} while (higherBeam != 1);
+        do {
+            higherBeam = fpga->getBeamValue(Top);
+            if (readSwitches() == 4) {
+                if (displayAbortDialog()) {
+                    pc.printf("INFO: Operation aborted form MBED.\n");
+                    displayPCStatus();
+                    return;
+                } else {
+                    lcd->cls();
+                    lcd->locate(0,0);
+                    lcd->printf("New haz block");
+                }
 
-		int colourValue[4];
-		rgbSensor.getAllColors(colourValue);
+            }
+        } while (higherBeam != 1);
 
-		do {
-			//			readyChar = pc.getc();
-			lowerBeam = fpga->checkForBlock();
-			// TODO: Check beam reading
-			//			if (readyChar != 'y'){
-			//				pc.printf("No block detected by beam\nTry again.");
-			//			}
-		} while (lowerBeam != 1);
-		higherBeam = fpga->checkForSize();
+        int colourValue[4];
+        rgbSensor.getAllColors(colourValue);
+
+        do {
+            lowerBeam = fpga->checkForBlock();
+        } while (lowerBeam != 1);
+        higherBeam = fpga->checkForSize();
 
-		_HazBlock = Block();
-		if (higherBeam == 1)
-			_HazBlock.size = Block::Big;
-		else if (higherBeam == 0)
-			_HazBlock.size = Block::Small;
-		for (int i = 0; i < 4; i++) {
-			_HazBlock.minColour.components[i] = colourValue[i] - ColourSensorError;
-			_HazBlock.maxColour.components[i] = colourValue[i] + ColourSensorError;
-		}
-		fpga->moveSortingServo(Haz);
-		fpga->moveStoppingServo(Go);
-		while (fpga->checkForBlock()) {}
-		fpga->moveStoppingServo(Stop);
-		fpga->moveSortingServo(NonHaz);
+        _HazBlock = Block();
+        if (higherBeam == 1)
+            _HazBlock.size = Block::Big;
+        else if (higherBeam == 0)
+            _HazBlock.size = Block::Small;
+        for (int i = 0; i < 3; i++) {
+            _HazBlock.minColour.components[i] = colourValue[i]/colourValue[3] - ColourSensorError;
+            _HazBlock.maxColour.components[i] = colourValue[i]/colourValue[3] + ColourSensorError;
+        }
+        fpga->moveSortingServo(Haz);
+        fpga->moveStoppingServo(Go);
+        while (fpga->checkForBlock()) {}
+        fpga->moveStoppingServo(Stop);
+        fpga->moveSortingServo(NonHaz);
 
-		pc.printf(	"HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
-	} else if (typeRaw == Query) {
-		pc.printf(	"HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
-	}
+        pc.printf(	"VALEU:HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
+    } else if (typeRaw == Query) {
+        pc.printf(	"VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
+    }
 }
 
 void getCurrentBlock(CommandTypeRaw typeRaw)
 {
-	pc.printf("DEBUG: Getting current block readings\n");
+    pc.printf("DEBUG: Getting current block readings\n");
 }
 
 void setIntegrationTimeTo(float integrationTime)
 {
-	rgbSensor.setIntegrationTime(integrationTime);
-	pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime);
+    rgbSensor.setIntegrationTime(integrationTime);
+    pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime);
 }
 
 void previewOnPC(bool on)
 {
-	pc.printf("setting preview on pc to %i.\n", on);
+    pc.printf("setting preview on pc to %i.\n", on);
 }
 
-void readColourSensor(){
-	int colourValue[4];
-	rgbSensor.getAllColors(colourValue);
-	pc.printf(	"Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
+void readColourSensor()
+{
+    int colourValue[4];
+    rgbSensor.getAllColors(colourValue);
+    pc.printf(	"VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
 
 }
 
-
 void testServos(Controls state)
 {
-	if (state == Start){
-		pc.printf("INFO: Running servo test.\n");
-		runServoTest = true;
-	}else if (state == Pause){
-		pc.printf("INFO: Finished running servo test.\n");
-		runServoTest = false;
-	}
+    if (state == Start) {
+        pc.printf("INFO: Running servo test.\n");
+        runServoTest = true;
+    } else if (state == Pause) {
+        pc.printf("INFO: Finished running servo test.\n");
+        runServoTest = false;
+    }
 }
 
 void resetServos()
 {
-	pc.printf("resetting servos.\n");
+    pc.printf("resetting servos.\n");
 }
 
 void getPortInfo()
 {
-	pc.printf("getting port info.\n");
+    pc.printf("getting port info.\n");
 }
 
 void setPortBaudRate(int baudRate)
 {
-	pc.baud(baudRate);
-	wait(0.1);
-	pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
+    pc.baud(baudRate);
+    wait(0.1);
+    pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
 }
 
-void setPortParity(int parity){
-	SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity);
-	pc.format(8, _parity, 1);
-	wait(0.1);
-	pc.printf("DEBUG: Setting port parity to: %i.\n", parity);
+void setPortParity(int parity)
+{
+    SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity);
+    pc.format(8, _parity, 1);
+    wait(0.1);
+    pc.printf("DEBUG: Setting port parity to: %i.\n", parity);
 }