Diff: main.cpp

Revision:

31:16e056b9f9e0

Parent:

30:c0bc92d009fe

Child:

32:9a4046224b11

--- a/main.cpp	Tue Dec 01 23:50:29 2015 +0000
+++ b/main.cpp	Thu Dec 03 15:50:14 2015 +0000
@@ -153,7 +153,7 @@
                             else if (runColourSensorTest == true)
                                 runInColourSensorTestMode();
                             else if (setNewHazBlock == true)
-                            	newHazBlockMode();
+                                newHazBlockMode();
                             if (i2cport->read_bit(11) == 1) {
                                 if (displayAbortDialog() == true) {
                                     currentMode = None;
@@ -199,9 +199,9 @@
                                             pc.printf(":<mbed>sort=pause;");
                                             currentState = Pause;
                                             continue;
-                                        }else if (connectedToPC == true && currentMode == None) {
-                                        	 goto setModeNone;
-                                        }else if (connectedToPC == false) {
+                                        } else if (connectedToPC == true && currentMode == None) {
+                                            goto setModeNone;
+                                        } else if (connectedToPC == false) {
                                             currentMode = None;
                                             currentState = Pause;
                                             abortOperation = true;
@@ -409,7 +409,7 @@
     pc.baud(baudRate);
     pc.format(8, SerialBase::None, gStopBits);
     pc.attach(&Rx_interrupt, Serial::RxIrq);
-	myLED4 = 1;
+    myLED4 = 1;
     pc.printf(":<pc>connect;");
 //    wait (0.3);
 //    while(recievingResponse == true) {}
@@ -419,8 +419,8 @@
 
 bool checkColour(int colourValues[])
 {
-    for (int i = 0; i < 3; i++){
-    	isBetweenHazValues[i] = false;
+    for (int i = 0; i < 3; i++) {
+        isBetweenHazValues[i] = false;
     }
     memset(adjustedValues, 0, sizeof(adjustedValues));
     memset(percentageError, 0, sizeof(percentageError));
@@ -439,20 +439,20 @@
     for (int i = 0; i < 3; i++) {
         adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
         percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
-		// Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set.
+    	// Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set.
         if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
             isHazColour[i] = true;
         }
     }
     */
     for (int i = 0; i < 3; i++) {
-            adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
-            percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];
+        adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
+        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)) {
-                isBetweenHazValues[i] = true;
-            }
+        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)) {
+            isBetweenHazValues[i] = true;
         }
+    }
 
     bool isHazBlock = false;
 
@@ -465,124 +465,139 @@
     return isHazBlock;
 }
 
-void newHazBlockMode(){
-	trySetHazBlockAgain:
-		pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
-        fpga->moveSortingServo(Haz);
+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;
+    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");
 
-        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");
-                }
+    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);
+    } while (higherBeam != 1 && setNewHazBlock == true && connectedToPC == true && currentMode == Maintanence);
+
+    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);
+    lowerBeam = fpga->getBeamValue(Bottom);
+    higherBeam = fpga->getBeamValue(Top);
+    pc.printf("DEBUG: Number of readings: %i\n",  readingCount);
+    if (lowerBeam != 1) {
+        lowerBeam = fpga->getBeamValue(Top);
+        while(lowerBeam != 1) {
+            lowerBeam = fpga->getBeamValue(Bottom);
         }
-        int blockSize = higherBeam;
+        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];
-        	}
+    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];
-        }
+    }
+    pc.printf("VALUE: Total: %i, %i, %i, %i \n:VALUE", totalComponents[0], totalComponents[1], totalComponents[2], totalComponents[3]);
+
+    float averageComponents[4];
+    memset(averageComponents, 0, sizeof(averageComponents));
+    for (int i = 0; i < 4; i++) {
+        averageComponents[i] = ((float)totalComponents[i] / (float)readingCount);
+    }
+    pc.printf("VALUE: Average: %i, %i, %i, %i \n:VALUE", averageComponents[0], averageComponents[1], averageComponents[2], averageComponents[3]);
+
+    float adjustedValues[3];
+    memset(adjustedValues, 0, sizeof(adjustedValues));
+    for (int i = 0; i < 3; i++) {
+        adjustedValues[i] = averageComponents[i] / averageComponents[3];
+    }
+    pc.printf("VALUE: Adjusted: %.3f, %.3f, %.3f \n:VALUE", adjustedValues[0], adjustedValues[1], adjustedValues[2]);
+
+    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++) {
+        pc.printf("DEBUG: Colour: %i\n", k);
+        for (int i = 0; i < 3; i++) {
+            percentageError[i] = (adjustedValues[i] - kAverageValues[k][i]) / kAverageValues[k][i];
+            pc.printf("DEBUG: Percenage error: %.5f\n", percentageError[i]);
 
-        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 ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[k][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[k][i] * errorMultiplier)) {
+                pc.printf("DEBUG: Pass\n");
+                matchesColour[i] = true;
+            }
         }
+        if (matchesColour[0] && matchesColour[1] && matchesColour[2]) {
+            detectedColour = static_cast<Block::BlockColour>(k);
+            pc.printf("DEBUG: Found match as: %i", detectedColour);
+            break;
+        } else {
+            matchesColour[0] = false;
+            matchesColour[1] = false;
+            matchesColour[2] = false;
+        }
+    }
 
-        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);
-        }
+    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 || setNewHazBlock == true) {
+                goto trySetHazBlockAgain;
+            }
+        } while (button != 2);
+    }
 
-        // Point and literal might not sync...
-        _HazBlock.size = static_cast<Block::Size>(blockSize);
-        _HazBlock.colour = detectedColour;
+    // 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("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);
+    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()