Revision 21:60c4fe94d79c, committed 2015-11-25

Comitter:

dreamselec

Date:

Wed Nov 25 19:21:12 2015 +0000

Parent:

20:4e0f0944f28f

Child:

22:993821a4c396

Commit message:

Optimised the code so, bottom servo is fast enough to sort the blocks. Code starts moving the servo after it detects block, checks the colour, then the size, if size and colour don't match then it moves the servo back to non haz.

Changed in this revision

--- a/fpga.cpp	Tue Nov 24 20:53:51 2015 +0000
+++ b/fpga.cpp	Wed Nov 25 19:21:12 2015 +0000
@@ -185,7 +185,7 @@
             CLK();          //Number finish
             bbOut.write(0);
 	}else if (position == NonHaz){
-		// NON-HAZ = 63
+		// NON-HAZ = 73
             bbRst.write(0);
             CLK();
             bbRst.write(1);
@@ -200,16 +200,16 @@
             CLK();
             bbOut.write(0);
             CLK();
-            bbOut.write(0);
+            bbOut.write(1);
+            CLK();
+            bbOut.write(1);
+            CLK();
+            bbOut.write(1);
             CLK();
             bbOut.write(1);
             CLK();
             bbOut.write(0);
             CLK();
-            bbOut.write(0);
-            CLK();
-            bbOut.write(1);
-            CLK();
             bbOut.write(0); 
             CLK();          //Position finish
             bbOut.write(0);

--- 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;
 }