Diff: main.cpp

Revision:

30:c0bc92d009fe

Parent:

29:9c0339e3c593

Child:

31:16e056b9f9e0

--- a/main.cpp	Tue Dec 01 22:56:08 2015 +0000
+++ b/main.cpp	Tue Dec 01 23:50:29 2015 +0000
@@ -33,7 +33,7 @@
 int 		blockCount = 0;
 bool 		lastBlockHaz = false;
 bool 		recievingResponse = false;
-bool 		isBetweenHazValues[3] = false;
+bool 		isBetweenHazValues[3] = { false, false, false };
 
 Commander 	_commander = Commander();
 Commander 	*commander = &_commander;
@@ -60,6 +60,7 @@
 void turnOffTopLEDs();
 void turnOffBottomLEDs();
 void runInColourSensorTestMode();
+void newHazBlockMode();
 
 int main()
 {
@@ -151,6 +152,8 @@
                                 runInBreakBeamTestMode();
                             else if (runColourSensorTest == true)
                                 runInColourSensorTestMode();
+                            else if (setNewHazBlock == true)
+                            	newHazBlockMode();
                             if (i2cport->read_bit(11) == 1) {
                                 if (displayAbortDialog() == true) {
                                     currentMode = None;
@@ -416,7 +419,9 @@
 
 bool checkColour(int colourValues[])
 {
-    isBetweenHazValues[3] = {false, false, false };
+    for (int i = 0; i < 3; i++){
+    	isBetweenHazValues[i] = false;
+    }
     memset(adjustedValues, 0, sizeof(adjustedValues));
     memset(percentageError, 0, sizeof(percentageError));
 
@@ -460,6 +465,126 @@
     return isHazBlock;
 }
 
+void newHazBlockMode(){
+	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);
+        }
+        int blockSize = higherBeam;
+
+        int totalComponents[4];
+        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[4];
+        memset(averageComponents, 0, sizeof(averageComponents));
+        for (int i = 0; i < 4; i++){
+        	averageComponents[i] = ((float)totalComponents[i] / (float)readingCount);
+        }
+		float adjustedValues[3];
+		memset(adjustedValues, 0, sizeof(adjustedValues));
+        for (int i = 0; i < 3; i++){
+        	adjustedValues[i] = averageComponents[i] / averageComponents[3];
+        }
+
+        Block::BlockColour detectedColour = Block::Wrong;
+        bool matchesColour[3] = {false, false, false};
+		
+		float percentageError[3];
+		memset(percentageError, 0, sizeof(percentageError));
+        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];
+				pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[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)) {
+        			pc.printf("DEBUG: Pass\n");
+        			matchesColour[i] = true;
+        		}
+        		if (matchesColour[0] && matchesColour[1] && matchesColour[2]){
+        			detectedColour = static_cast<Block::BlockColour>(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 = static_cast<Block::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);
+}
+
 void printPCDetectedText()
 {
     lcd->cls();