Diff: globals.cpp

Revision:

27:2cb1bdb7ae3d

Parent:

26:bbcc25418ffa

Child:

28:7e4d29977d72

--- a/globals.cpp	Mon Nov 30 21:29:51 2015 +0000
+++ b/globals.cpp	Tue Dec 01 16:16:26 2015 +0000
@@ -25,6 +25,7 @@
 bool getColourSensorValue = false;
 bool getBlockColourValue = false;
 int errorMultiplier = 2;
+int hazReadingCount = 1;
 
 PCModes currentMode = None;
 Controls currentState = Pause;
@@ -56,7 +57,7 @@
 	defaultHazBlock.size = Block::Small;
 	
 	_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]);
+//	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){
@@ -108,6 +109,7 @@
 void hazBlock(CommandTypeRaw typeRaw)
 {
     if (typeRaw == Set) {
+        fpga->moveSortingServo(Haz);
         pc.printf("INFO:Setting new haz block.\n");
         int lowerBeam = 0;
         int higherBeam = 0;
@@ -116,12 +118,15 @@
         lcd->cls();
         lcd->locate(0,0);
         lcd->printf("New haz block");
-
+        
         do {
             higherBeam = fpga->getBeamValue(Top);
             if (readSwitches() == 4) {
                 if (displayAbortDialog()) {
+                    //TODO: tell pc
+                    pc.printf(":<mbed>haz-block=pause");
                     pc.printf("INFO: Operation aborted form MBED.\n");
+                    fpga->moveSortingServo(NonHaz);
                     displayPCStatus();
                     return;
                 } else {
@@ -132,6 +137,21 @@
 
             }
         } while (higherBeam != 1);
+        
+        int colourValues[4];
+        
+        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;
+    }
 
         int colourValue[4];
         rgbSensor.getAllColors(colourValue);