Diff: main.cpp

Revision:

22:993821a4c396

Parent:

21:60c4fe94d79c

Child:

23:db91aaa43a9e

--- a/main.cpp	Wed Nov 25 19:21:12 2015 +0000
+++ b/main.cpp	Thu Nov 26 15:47:17 2015 +0000
@@ -270,21 +270,30 @@
     fpga->moveStoppingServo(Go);
 
     int colourValues[4];
-    rgbSensor.getAllColors(colourValues);
+    int averageColourValues[4] = {0, 0, 0, 0};
+    for (int i = 0; i < 3; 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] / 3;
+    }
 
     bool haz = false;
     haz = checkColour(colourValues);
-
+    
     if (!haz) {
         fpga->moveSortingServo(NonHaz);
     }
-
-    while (fpga->checkForBlock() == 0) { }
-    int blockSize;
-    if (fpga->checkForSize() == HazBlock->size && haz) {
+	int blockSize;
+    while (fpga->checkForBlock() == 0) {  }
+    blockSize = fpga->checkForSize();
+    if (blockSize == HazBlock->size && haz) {
 //        fpga->moveSortingServo(Haz);
 //        fpga->moveStoppingServo(Go);
-        int blockSize = HazBlock->size;
+//        blockSize = HazBlock->size;
         while(fpga->getBeamValue(Bottom) == 1) {}
         wait(kServoWait);
         fpga->moveStoppingServo(Stop);
@@ -300,8 +309,8 @@
             if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2))
             	pc.printf("DEBUG:%i.\n", i);
         }
-        pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], haz);
-        pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
+        pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
+//        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
     }
 
     // Re-Attach rx interrupt
@@ -385,6 +394,7 @@
     } else {
         isHazBlock = false;
     }
+//    pc.printf("DEBUG:haz: %i\n", isHazBlock);
     return isHazBlock;
 }