Diff: main.cpp

Revision:

20:4e0f0944f28f

Parent:

19:61b21ac4896e

Child:

21:60c4fe94d79c

diff -r 61b21ac4896e -r 4e0f0944f28f main.cpp
--- a/main.cpp	Tue Nov 24 01:38:46 2015 +0000
+++ b/main.cpp	Tue Nov 24 20:53:51 2015 +0000
@@ -24,7 +24,6 @@
 
 MCP23017 *i2cport;
 WattBob_TextLCD *lcd;
-
 TCS3472_I2C rgbSensor(p28, p27);
 Serial      pc(USBTX, USBRX);
 uint8_t     rxBuffer[kSmallBufferSize + 1];
@@ -35,6 +34,7 @@
 FPGA 		_fpga = FPGA();
 extern FPGA		*fpga = &_fpga;
 
+Block defualtHazBlock = Block();
 extern Block _HazBlock;
 Block *HazBlock = &_HazBlock;
 
@@ -43,7 +43,7 @@
 void initInternal();
 void initPort(int baudRate=kDefaultBaudRate);
 void printPCDetectedText();
-bool displayAbortDialog();
+//bool displayAbortDialog();
 void Rx_interrupt();
 bool checkColour(int colourValues[]);
 void runInServoTestMode();
@@ -59,10 +59,9 @@
     srand((unsigned)time(NULL));
     U_LEDS_OFF();
     lcd->cls();
-    myLED1 = 1;
 
     rgbSensor.enablePowerAndRGBC();
-    rgbSensor.setIntegrationTime(2.4);
+    rgbSensor.setIntegrationTime(3);
 
     // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
     // https://developer.mbed.org/cookbook/Serial-Interrupts
@@ -141,24 +140,30 @@
                                 lcd->locate(1,0);
                                 lcd->printf("1: Start");
                                 int button = 0;
-                                while (currentState == Pause) {
+                                i2cport->write_bit(1, 12);
+                                i2cport->write_bit(1, 15);
+                                while (currentState == Pause && currentMode == Normal) {
                                     button = readSwitches();
                                     if (button == 1) {
                                         pc.printf("state:start\n");
                                         currentState = Start;
                                     } else if (button == 4) {
-                                        goto setModeNone;
+                                        currentMode = None;
+//                                        goto setModeNone;
+                                        break;
                                     }
                                 }
                             }
                             if (currentState == Start) {
-                                while (currentState == Start) {
+                                lcd->cls();
+                                lcd->locate(0,0);
+                                lcd->printf("Sorting mode...");
+                                while (currentState == Start && currentMode == Normal) {
                                     if (waitForBlock() == false) {
-                                        currentState = Pause;
                                         if (connectedToPC == true) {
                                             // TODO: Tell PC to update UI if aborted from MBED.
                                             pc.printf("state:pause\n");
-
+                                            currentState = Pause;
                                             continue;
                                         } else if (connectedToPC == false) {
                                             currentMode = None;
@@ -172,9 +177,18 @@
                         }
                     } else if (currentMode == None) {
 setModeNone:
+                        D_LEDS_OFF();
+                        i2cport->write_bit(1,15);
                         displayPCStatus();
-                        while (currentMode == None && abortOperation == false) {
-                            abortOperation = readSwitches() == 4;
+                        while (currentMode == None && abortOperation == false && connectedToPC == true) {
+                            if (i2cport->read_bit(11)) {
+                                abortOperation = displayAbortDialog();
+                                // Cancel the Abort
+                                if (abortOperation == false) {
+                                    displayPCStatus();
+                                    i2cport->write_bit(1, 15);
+                                }
+                            }
                         }
                     }
                 }
@@ -194,6 +208,8 @@
 // true if block detected, false if cancelled/connected to PC.
 bool waitForBlock()
 {
+    myLED4 = 0;
+    myLED1 = 1;
     if (connectedToPC == false) {
         bool abortOperation = false;
         int blockInserted = 0;
@@ -232,9 +248,9 @@
                     i2cport->write_bit(1, 15);
                 }
             }
-        } while (abortOperation == false && blockInserted != 1 && connectedToPC == true);
+        } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
 
-        if (abortOperation == true || connectedToPC == false)
+        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
             return false;
         else
             return true;
@@ -243,6 +259,8 @@
 
 void sortBlock()
 {
+    myLED1 = 0;
+    myLED2 = 1;
     // Cannot Abort any longer. Block is inserted.
     // Detach rx interrupt until block processed.
     NVIC_DisableIRQ(UART1_IRQn);
@@ -257,7 +275,6 @@
     }
 
     blockSize = fpga->checkForSize();
-    myLED3 = blockSize;
     bool haz = false;
 
     if (blockSize == HazBlock->size) {
@@ -268,13 +285,14 @@
         fpga->moveStoppingServo(Stop);
     }
 
+
     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);
 
-    myLED3 = 0;
     // Re-Attach rx interrupt
     NVIC_EnableIRQ(UART1_IRQn);
-
+    myLED3 = 0;
+    myLED4 = 1;
     return;
 }
 
@@ -300,13 +318,14 @@
 
 void initInternal()
 {
+    myLED1 = 1;
     i2cport = new MCP23017(p9, p10, 0x40);
     lcd = new WattBob_TextLCD(i2cport);
-    myLED1 = 1;
     BACKLIGHT_ON(i2cport);
     lcd->cls();
-    LCDFL();
+    lcd->locate(0,0);
     lcd->printf("Initilizing...");
+    DefaultHazBlock();
     myLED2 = 1;
     return;
 }
@@ -323,37 +342,53 @@
 
 bool checkColour(int colourValues[])
 {
-    bool isHazColour[4] = {false, false, false, false };
+    myLED2 = 0;
+    myLED3 = 1;
+    bool isHazColour[3] = {false, false, false };
+
+    float adjustedValues[3];
+    float percentageError[3];
+
 
-    for (int i = 0; i < 4; i++) {
-        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
+    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);
             isHazColour[i] = true;
+        } else if (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2) {
+            isHazColour[i] = true;
+            pc.printf("DEBUG:%i.\n", i);
         }
     }
 
-    fpga->moveSortingServo(Haz);
-    fpga->moveStoppingServo(Go);
-    while(fpga->checkForBlock()) {}
-    fpga->moveStoppingServo(Stop);
-    wait(0.5);
-    fpga->moveSortingServo(NonHaz);
-    return true;
+//    for (int i = 0; i < 3; i++) {
+//        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
+//            isHazColour[i] = true;
+//        }
+//    }
 
     bool isHazBlock = false;
 
-    if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]) {
+    if (isHazColour[0] && isHazColour[1] && isHazColour[2]) {
         fpga->moveSortingServo(Haz);
         fpga->moveStoppingServo(Go);
         while(fpga->checkForBlock()) {}
+        wait(kServoWait);
         fpga->moveStoppingServo(Stop);
         fpga->moveSortingServo(NonHaz);
-        return true;
+        isHazBlock = true;
     } else {
         fpga->moveStoppingServo(Go);
         while(fpga->checkForBlock()) {}
         fpga->moveStoppingServo(Stop);
-        return false;
+        isHazBlock = false;
     }
+
+    pc.printf("BLOCK:Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i;", adjustedValues[0], adjustedValues[1], adjustedValues[2], isHazBlock);
+    return isHazBlock;
 }
 
 void printPCDetectedText()
@@ -415,11 +450,11 @@
         if (button == 1) {
             fpga->toggleStoppingServo();
             printServoInfoOnLCD();
-            wait(0.5);
+            wait(kServoWait);
         } else if (button == 2) {
             fpga->toggleSortingServo();
             printServoInfoOnLCD();
-            wait(0.5);
+            wait(kServoWait);
         }
         finished = button == 4;
         button = 0;