Diff: globals.cpp

Revision:

26:bbcc25418ffa

Parent:

25:792540d69c49

Child:

27:2cb1bdb7ae3d

--- a/globals.cpp	Mon Nov 30 15:06:38 2015 +0000
+++ b/globals.cpp	Mon Nov 30 21:29:51 2015 +0000
@@ -12,7 +12,10 @@
 int gStopBits = 1;
 float gIntegrationTime = 3;
 int gToggleServoNumber = 0;
+float currentMinError[3] = {0,0,0};
+float currentMaxError[3] = {0,0,0};
 
+Block defaultHazBlock = Block();
 Block _HazBlock = Block();
 
 bool connectedToPC = false;
@@ -21,10 +24,17 @@
 bool runColourSensorTest = false;
 bool getColourSensorValue = false;
 bool getBlockColourValue = false;
+int errorMultiplier = 2;
+
 PCModes currentMode = None;
 Controls currentState = Pause;
 
 void DefaultHazBlock(){
+	
+	for (int i = 0; i < 3; i++){ 
+		currentMaxError[i] = kMaxError[i];
+		currentMinError[i] = kMinError[i];
+	}
 	Colour _minRedBlock = Colour();
 	Colour _maxRedBlock = Colour();
 	Colour _averageRedBlock = Colour();
@@ -35,16 +45,31 @@
 		_averageRedBlock.components[i] = kAverageRedBlock[i];
 	}
 	
-	_HazBlock.minColour = Colour(_minRedBlock);
-	_HazBlock.maxColour = Colour(_maxRedBlock);
-	_HazBlock.averageColour = Colour(_averageRedBlock);
-	_HazBlock.size = Block::Small;
+//	_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);
+	defaultHazBlock.size = Block::Small;
 	
-//	defaultHazBlock.minColour = Colour(_minRedBlock);
-//	defaultHazBlock.maxColour = Colour(_maxRedBlock);
-//	defaultHazBlock.averageColour = Colour(_averageRedBlock);
-	
-//	Block _HazBlock = Block(defaultHazBlock);
+	_HazBlock = Block(defaultHazBlock);
+	pc.printf(	"VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%f,%f,%f,%f\n \t Max Colour:%f,%f,%f,%f: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 printColourDescription(Colour colour){
+	pc.printf("Red: %.3f, Green: %.3f, Blue: %.3f, Clear: %.3f\n", colour.components[0], colour.components[1], colour.components[2], colour.components[3]);
+}
+
+void printBlockDescription(Block block){
+	pc.printf("VALUE:Size: %i\n", block.size);
+	printColourDescription(block.minColour);
+	printColourDescription(block.averageColour);
+	printColourDescription(block.maxColour);
+	//TODO: print errors
+	pc.printf(":VALUE");
 }
 
 int readSwitches()
@@ -131,6 +156,7 @@
         fpga->moveStoppingServo(Stop);
         fpga->moveSortingServo(NonHaz);
 
+		//TODO: Fix format.
         pc.printf(	"VALUE:HazBlock:\n \t Size:%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]);