Diff: globals.cpp

Revision:

29:9c0339e3c593

Parent:

28:7e4d29977d72

Child:

30:c0bc92d009fe

diff -r 7e4d29977d72 -r 9c0339e3c593 globals.cpp
--- a/globals.cpp	Tue Dec 01 20:54:26 2015 +0000
+++ b/globals.cpp	Tue Dec 01 22:56:08 2015 +0000
@@ -15,8 +15,8 @@
 float currentMinError[3] = {0,0,0};
 float currentMaxError[3] = {0,0,0};
 
-Block defaultHazBlock = Block();
-Block _HazBlock = Block();
+Block defaultHazBlock = Block(Block::Small, Block::Red);
+Block _HazBlock = Block(defaultHazBlock);
 
 bool connectedToPC = false;
 bool runServoTest = false;
@@ -24,6 +24,7 @@
 bool runColourSensorTest = false;
 bool getColourSensorValue = false;
 bool getBlockColourValue = false;
+bool setNewHazBlock = false;
 bool pcModeChanged = false;
 int errorMultiplier = 1;
 int hazReadingCount = 1;
@@ -34,8 +35,8 @@
 void DefaultHazBlock(){
 	
 	for (int i = 0; i < 3; i++){ 
-		currentMaxError[i] = kMaxError[i];
-		currentMinError[i] = kMinError[i];
+		currentMaxError[i] = kMaxRedError[i];
+		currentMinError[i] = kMinRedError[i];
 	}
 	Colour _minRedBlock = Colour();
 	Colour _maxRedBlock = Colour();
@@ -110,11 +111,17 @@
 void hazBlock(CommandTypeRaw typeRaw)
 {
     if (typeRaw == Set) {
+trySetHazBlockAgain:
+		pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
         fpga->moveSortingServo(Haz);
         pc.printf("INFO:Setting new haz block.\n");
         int lowerBeam = 0;
         int higherBeam = 0;
-        char readyChar = '\0';
+        int colourValues[6][4];
+        for (int i = 0; i < 6; i++){
+        	memset(colourValues[i], 0, sizeof(colourValues[6]));
+        }
+        int readingCount = 0;
 
         lcd->cls();
         lcd->locate(0,0);
@@ -125,7 +132,7 @@
             if (readSwitches() == 4) {
                 if (displayAbortDialog()) {
                     //TODO: tell pc
-                    pc.printf(":<mbed>haz-block=pause");
+                    pc.printf(":<mbed>haz-block=pause;");
                     pc.printf("INFO: Operation aborted form MBED.\n");
                     fpga->moveSortingServo(NonHaz);
                     displayPCStatus();
@@ -135,50 +142,91 @@
                     lcd->locate(0,0);
                     lcd->printf("New haz block");
                 }
+            }
+        } while (higherBeam != 1 && setNewHazBlock == true);
 
-            }
-        } while (higherBeam != 1);
+        if (setNewHazBlock == false) {
+        	displayPCStatus();
+        	return;
+        }
         
-        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);
-
         do {
-            lowerBeam = fpga->checkForBlock();
-        } while (lowerBeam != 1);
-        higherBeam = fpga->checkForSize();
+        	rgbSensor.getAllColors(colourValues[readingCount]);
+        	readingCount++;
+        } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0);
+
+        lowerBeam = fpga->getBeamValue(Bottom);
+        higherBeam = fpga->getBeamValue(Top);
+        if (lowerBeam != 1){
+        	lowerBeam = fpga->getBeamValue(Top);
+        	while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); }
+        	higherBeam = fpga->getBeamValue(Bottom);
+        }
+        Block::Size blockSize = higherBeam;
+
+        int totalComponents[3];
+        memset(totalComponents, 0, sizeof(totalComponents));
+        for (int k = 0; k < readingCount; k++){
+        	for (int i = 0; i < 4; i++){
+        		totalComponents[i] += colourValues[k][i];
+        	}
+        }
+        float averageComponents[3];
+        memset(averageComponents, 0, sizeof(averageComponents));
+
+        for (int i = 0; i < 4; i++){
+        	averageComponents = (float)totalComponents[i] / (float)readingCount;
+        }
+
+        for (int i = 0; i < 3; i++){
+        	adjustedValues[i] = averageComponents[i] / averageComponents[i];
+        }
+
+        Block::BlockColour detectedColour = Block::Wrong;
+        bool matchesColour[3] = {false, false, false};
+
+        for (int k = 0; k < 7; k++){
+        	for (int i = 0; i < 3; i++) {
+        		percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];
 
-        _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;
+        		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) {
+        			matchesColour[i] = true;
+        		}
+        		if (matchesColour[0] && matchesColour[1] && matchesColour[2]){
+        			detectedColour = k;
+        			break;
+        		}
+        	}
         }
-        fpga->moveSortingServo(Haz);
-        fpga->moveStoppingServo(Go);
-        while (fpga->checkForBlock()) {}
-        fpga->moveStoppingServo(Stop);
+
+        if (detectedColour != Block::Wrong){
+        	pc.printf("ERROR: Could not detect colour.\n");
+        	lcd->cls();
+        	lcd->printf("1: Try again");
+        	lcd->locate(1,0);
+        	lcd->printf("2: Revert to last");
+        	int button = 0;
+        	do {
+        		button = readSwitches();
+        		if (button == 1){
+        			goto trySetHazBlockAgain;
+        		}
+        	} while (button != 2);
+        }
+
+        // Point and literal might not sync...
+        _HazBlock.size = blockSize;
+        _HazBlock.colour = detectedColour;
+
+        pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
+
+
+        pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", HazBlock->size, kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][2], kMaxError[HazBlock->colour][0], kMaxError[HazBlock->colour][1], kMaxError[HazBlock->colour][2]);
+        pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[HazBlock->colour][0], kAverageValues[HazBlock->colour][1], kAverageValues[HazBlock->colour][2], kAverageValues[HazBlock->colour][3]);
         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]);
+    	pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", HazBlock->size, kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][2], kMaxError[HazBlock->colour][0], kMaxError[HazBlock->colour][1], kMaxError[HazBlock->colour][2]);
+    	pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[HazBlock->colour][0], kAverageValues[HazBlock->colour][1], kAverageValues[HazBlock->colour][2], kAverageValues[HazBlock->colour][3]);
     }
 }