Diff: main.cpp

Revision:

28:7e4d29977d72

Parent:

27:2cb1bdb7ae3d

Child:

29:9c0339e3c593

diff -r 2cb1bdb7ae3d -r 7e4d29977d72 main.cpp
--- a/main.cpp	Tue Dec 01 16:16:26 2015 +0000
+++ b/main.cpp	Tue Dec 01 20:54:26 2015 +0000
@@ -32,6 +32,7 @@
 float 		adjustedValues[3];
 int 		blockCount = 0;
 bool 		lastBlockHaz = false;
+bool 		recievingResponse = false;
 
 Commander 	_commander = Commander();
 Commander 	*commander = &_commander;
@@ -63,25 +64,10 @@
 {
     initInternal();
     initPort();
-    srand((unsigned)time(NULL));
-    U_LEDS_OFF();
-    lcd->cls();
-
-    rgbSensor.enablePowerAndRGBC();
-    rgbSensor.setIntegrationTime(gIntegrationTime);
-
-    // 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
-    pc.attach(&Rx_interrupt, Serial::RxIrq);
-
-    DefaultHazBlock();
-    fpga->moveStoppingServo(Stop);
-    fpga->moveSortingServo(NonHaz);
-    pc.printf(":<pc>connect;");
-    wait(0.2);
 
     for (;;) {
         if (connectedToPC == false) {
+            pc.printf("DEBUG: PC did not responded.\n");
             lcd->cls();
             i2cport->write_bit(1, 12);
             lcd->locate(0,0);
@@ -204,13 +190,16 @@
                                 lcd->printf("Sorting mode...");
                                 while (currentState == Start && currentMode == Normal) {
                                     if (waitForBlock() == false) {
-                                        if (connectedToPC == true && currentState != Pause) {
+                                        if (connectedToPC == true && currentState != Pause && currentMode == Normal) {
                                             // TODO: Tell PC to update UI if aborted from MBED.
                                             pc.printf(":<mbed>sort=pause;");
                                             currentState = Pause;
                                             continue;
-                                        } else if (connectedToPC == false) {
+                                        }else if (connectedToPC == true && currentMode == None) {
+                                        	 goto setModeNone;
+                                        }else if (connectedToPC == false) {
                                             currentMode = None;
+                                            currentState = Pause;
                                             abortOperation = true;
                                         }
                                     } else {
@@ -266,8 +255,6 @@
                 // Cancel the Abort
                 if (abortOperation == false) {
                     displayWaitingLine();
-                    lcd->printf("for block");
-                    i2cport->write_bit(1, 15);
                 }
             }
         } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
@@ -280,10 +267,11 @@
         bool abortOperation = false;
         int blockInserted = 0;
         // Wait until a block is breaking the beam, or button 4 is pressed to abort.
+        pc.printf("INFO: Waiting for block.\n");
         do {
             blockInserted = fpga->getBeamValue(Top);
             myLED4 = blockInserted;
-            if (i2cport->read_bit(11)) {
+            if (readSwitches() == 4) {
                 abortOperation = displayAbortDialog();
                 // Cancel the Abort
                 if (abortOperation == false) {
@@ -294,7 +282,7 @@
             }
         } while (abortOperation == false && blockInserted != 1 && connectedToPC == true && currentState == Start && currentMode == Normal);
 
-        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode == Maintanence)
+        if (abortOperation == true || connectedToPC == false || currentState == Pause || currentMode != Normal)
             return false;
         else
             return true;
@@ -304,6 +292,7 @@
 
 void sortBlock()
 {
+    pc.printf("BLOCK: Sorting block");
     myLED1 = 0;
     myLED2 = 1;
     // Cannot Abort any longer. Block is inserted.
@@ -327,6 +316,7 @@
 
     lastBlockHaz = false;
     lastBlockHaz = checkColour(averageColourValues);
+//	lastBlockHaz = checkColour(colourValues);
 
     if (!lastBlockHaz) {
         fpga->moveSortingServo(NonHaz);
@@ -371,6 +361,7 @@
 /// Called every-time it receives an char from PC.
 void Rx_interrupt()
 {
+    recievingResponse = true;
     char interruptChar = pc.getc();
     // Uncomment to Echo to USB serial to watch data flow
     //    pc.putc(interruptChar);
@@ -386,6 +377,7 @@
     }
 
     NVIC_EnableIRQ(UART1_IRQn);
+    recievingResponse = false;
 }
 
 void initInternal()
@@ -398,7 +390,12 @@
     lcd->locate(0,0);
     lcd->printf("Initilizing...");
     DefaultHazBlock();
+    rgbSensor.enablePowerAndRGBC();
+    rgbSensor.setIntegrationTime(gIntegrationTime);
+    srand((unsigned)time(NULL));
     myLED2 = 1;
+    fpga->moveStoppingServo(Stop);
+    fpga->moveSortingServo(NonHaz);
     return;
 }
 
@@ -407,8 +404,12 @@
     myLED3 = 1;
     pc.baud(baudRate);
     pc.format(8, SerialBase::None, gStopBits);
-    myLED4 = 1;
-    wait (0.1);
+    pc.attach(&Rx_interrupt, Serial::RxIrq);
+	myLED4 = 1;
+    pc.printf(":<pc>connect;");
+//    wait (0.3);
+//    while(recievingResponse == true) {}
+    turnOffTopLEDs();
     return;
 }
 
@@ -561,6 +562,7 @@
     lcd->cls();
     lcd->locate(0,0);
     lcd->printf("Done servo test");
+    fpga->moveSortingServo(NonHaz);
     if (runServoTest == true) {
         pc.printf(":<servos>test=pause;");
         runServoTest = false;
@@ -695,6 +697,7 @@
 
             do {
                 rgbSensor.getAllColors(colourValues[valueCount]);
+                valueCount++;
             } while (fpga->getBeamValue(Top) == 1 && valueCount < 3);
 
             turnOffTopLEDs();