Diff: globals.cpp

Revision:

30:c0bc92d009fe

Parent:

29:9c0339e3c593

Child:

32:9a4046224b11

--- a/globals.cpp	Tue Dec 01 22:56:08 2015 +0000
+++ b/globals.cpp	Tue Dec 01 23:50:29 2015 +0000
@@ -111,122 +111,11 @@
 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;
-        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);
-        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 {
-                    lcd->cls();
-                    lcd->locate(0,0);
-                    lcd->printf("New haz block");
-                }
-            }
-        } while (higherBeam != 1 && setNewHazBlock == true);
-
-        if (setNewHazBlock == false) {
-        	displayPCStatus();
-        	return;
-        }
-        
-        do {
-        	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];
-
-        		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;
-        		}
-        	}
-        }
-
-        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);
-    } else if (typeRaw == Query) {
-    	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]);
+    	setNewHazBlock = true;
+    	pc.printf("INFO: Setting new haz block.\n");
+	} else if (typeRaw == Query) {
+    	pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%.3f, %.3f, %.3f\n \t Max Error:%.3f, %.3f, %.3f\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]);
     }
 }
 
@@ -239,7 +128,7 @@
 {
     gIntegrationTime = integrationTime;
     rgbSensor.setIntegrationTime(gIntegrationTime);
-    pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime);
+    pc.printf("DEBUG: Setting integration-time to %.2f.\n", gIntegrationTime);
 }
 
 void previewOnPC(bool on)