Diff: main.cpp

Revision:

21:60c4fe94d79c

Parent:

20:4e0f0944f28f

Child:

22:993821a4c396

--- a/main.cpp	Tue Nov 24 20:53:51 2015 +0000
+++ b/main.cpp	Wed Nov 25 19:21:12 2015 +0000
@@ -28,6 +28,8 @@
 Serial      pc(USBTX, USBRX);
 uint8_t     rxBuffer[kSmallBufferSize + 1];
 int 		rxIndex = 0;
+float 		percentageError[3];
+float 		adjustedValues[3];
 
 Commander 	_commander = Commander();
 Commander 	*commander = &_commander;
@@ -264,30 +266,43 @@
     // Cannot Abort any longer. Block is inserted.
     // Detach rx interrupt until block processed.
     NVIC_DisableIRQ(UART1_IRQn);
+    fpga->moveSortingServo(Haz);
+    fpga->moveStoppingServo(Go);
+
     int colourValues[4];
     rgbSensor.getAllColors(colourValues);
 
-    int canCheckForSize = fpga->checkForBlock();
-    int blockSize = 0;
+    bool haz = false;
+    haz = checkColour(colourValues);
 
-    while (canCheckForSize == 0) {
-        canCheckForSize = fpga->checkForBlock();
+    if (!haz) {
+        fpga->moveSortingServo(NonHaz);
     }
 
-    blockSize = fpga->checkForSize();
-    bool haz = false;
-
-    if (blockSize == HazBlock->size) {
-        haz = checkColour(colourValues);
+    while (fpga->checkForBlock() == 0) { }
+    int blockSize;
+    if (fpga->checkForSize() == HazBlock->size && haz) {
+//        fpga->moveSortingServo(Haz);
+//        fpga->moveStoppingServo(Go);
+        int blockSize = HazBlock->size;
+        while(fpga->getBeamValue(Bottom) == 1) {}
+        wait(kServoWait);
+        fpga->moveStoppingServo(Stop);
     } else {
-        fpga->moveStoppingServo(Go);
-        while(fpga->checkForSize()) {}
-        fpga->moveStoppingServo(Stop);
+        haz = false;
     }
-
+    fpga->moveSortingServo(NonHaz);
+    while(fpga->checkForSize()) {}
 
-    if (connectedToPC)
-        pc.printf("BLOCK:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i,Haz:%i;", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
+    if (connectedToPC) {
+        for (int i = 0; i < 3; i++) {
+            pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
+            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);
+    }
 
     // Re-Attach rx interrupt
     NVIC_EnableIRQ(UART1_IRQn);
@@ -345,22 +360,15 @@
     myLED2 = 0;
     myLED3 = 1;
     bool isHazColour[3] = {false, false, false };
-
-    float adjustedValues[3];
-    float percentageError[3];
-
+    memset(adjustedValues, 0, sizeof(adjustedValues));
+    memset(percentageError, 0, sizeof(percentageError));
 
     for (int i = 0; i < 3; i++) {
         adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
         percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
-        pc.printf("DEBUG:Percentage Error: %.5f.\n", percentageError[i]);
 
-        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0) {
-            pc.printf("DEBUG:%i.\n", i);
+        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
             isHazColour[i] = true;
-        } else if (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2) {
-            isHazColour[i] = true;
-            pc.printf("DEBUG:%i.\n", i);
         }
     }
 
@@ -373,21 +381,10 @@
     bool isHazBlock = false;
 
     if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
-        fpga->moveSortingServo(Haz);
-        fpga->moveStoppingServo(Go);
-        while(fpga->checkForBlock()) {}
-        wait(kServoWait);
-        fpga->moveStoppingServo(Stop);
-        fpga->moveSortingServo(NonHaz);
         isHazBlock = true;
     } else {
-        fpga->moveStoppingServo(Go);
-        while(fpga->checkForBlock()) {}
-        fpga->moveStoppingServo(Stop);
         isHazBlock = false;
     }
-
-    pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], isHazBlock);
     return isHazBlock;
 }