Diff: main.cpp

Revision:

15:777390eb5afd

Parent:

14:cf2f255b5560

Child:

17:af373246bf80

--- a/main.cpp	Sun Nov 22 17:53:36 2015 +0000
+++ b/main.cpp	Sun Nov 22 20:29:08 2015 +0000
@@ -46,13 +46,8 @@
 void Rx_interrupt();
 void sortBlock(int colourValues[]);
 void runInServoTestMode();
-
-void displayWaitingLine(){
-	lcd->cls();
-	lcd->locate(0,0);
-    lcd->printf("Waiting...");
-    lcd->locate(1,0);
-}
+void displayWaitingLine();
+void displayPCStatus();
 
 int main()
 {
@@ -62,7 +57,7 @@
     U_LEDS_OFF();
     lcd->cls();
     myLED1 = 1;
-    
+
     rgbSensor.enablePowerAndRGBC();
     rgbSensor.setIntegrationTime(100);
 
@@ -77,7 +72,7 @@
         LCDSL();
         i2cport->write_bit(1,13);
         lcd->printf("2: Connect to PC");
-        
+
         fpga->moveStoppingServo(Stop);
         fpga->moveSortingServo(NonHaz);
 
@@ -99,7 +94,6 @@
             bool abortOperation = false;
             for(;;) {
                 displayWaitingLine();
-                lcd->locate(1, 0);
                 lcd->printf("for block.");
 
                 i2cport->write_bit(1, 15);
@@ -124,7 +118,7 @@
                     D_LEDS_OFF();
                     break;
                 }
-				
+
                 // Cannot Abort any longer. Block is inserted.
                 // Detach rx interrupt until block processed.
                 NVIC_DisableIRQ(UART1_IRQn);
@@ -133,7 +127,7 @@
                 int blockSize = 0;
 
                 while (canCheckForSize == 0) {
-                	canCheckForSize = fpga->checkForBlock();
+                    canCheckForSize = fpga->checkForBlock();
                 }
 
                 blockSize = fpga->checkForSize();
@@ -148,13 +142,13 @@
 //                    lcd->locate(0,0);
 //                    lcd->printf("Sorting");
                     sortBlock(colourValues);
-                }else {
-                	fpga->moveStoppingServo(Go);
-                	while(fpga->checkForSize()){}
-                	fpga->moveStoppingServo(Stop);
+                } else {
+                    fpga->moveStoppingServo(Go);
+                    while(fpga->checkForSize()) {}
+                    fpga->moveStoppingServo(Stop);
                 }
-				
-				myLED3 = 0;
+
+                myLED3 = 0;
                 // Re-Attach rx interrupt
                 NVIC_EnableIRQ(UART1_IRQn);
             }
@@ -163,17 +157,8 @@
 
         if (selection == 2 || connectedToPC == true) {
             for (;;) {
-                if (!connectedToPC) {
-                    i2cport->write_bit(1, 13);
-                    // Wait for PC to send '!<pc>connect;' command.
-                    lcd->cls();
-                    LCDFL();
-                    lcd->printf("Waiting for PC...");
-                    wait(0.5);
-                    D_LEDS_OFF();
-                    LCDSL();
-                    lcd->printf("4: Main Menu");
-                }
+                displayPCStatus();
+                
                 i2cport->write_bit(1, 15);
                 int abortOperation = false;
                 while (connectedToPC == false && abortOperation == false) {
@@ -184,18 +169,18 @@
                     D_LEDS_OFF();
                     break;
                 }
-
+                
+				displayPCStatus();
                 while (abortOperation == false && connectedToPC == true) {
-					if (runServoTest == true){
-                    	runInServoTestMode();
-                    	lcd->cls();
-                    	lcd->locate(0,0);
-                    	lcd->printf("Connected to PC");
-                    	lcd->locate(1,0);
-                    	lcd->printf("4:Disconnect");
-                    	i2cport->write_bit(1,15);
+                    if (runServoTest == true)
+                        runInServoTestMode();
+
+                    if (readSwitches() == 4) {
+                        abortOperation = displayAbortDialog();
+                        if (abortOperation == false)
+                            displayPCStatus();
+
                     }
-                    abortOperation = readSwitches() == 4;
                 }
 
                 if (abortOperation == true ) {
@@ -252,36 +237,37 @@
     return;
 }
 
-void sortBlock(int colourValues[]){
-	bool isHazColour[4] = {false, false, false, false };
-	
-	for (int i = 0; i < 4; i++){
-		if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]){
-			isHazColour[i] = true;
-		}
-	}
-	
-	fpga->moveSortingServo(Haz);
-	fpga->moveStoppingServo(Go);
-	while(fpga->checkForBlock()) {}
-	fpga->moveStoppingServo(Stop);
-	wait(0.5);
-	fpga->moveSortingServo(NonHaz);
-	return;
-	
-	bool isHazBlock = false;
-	
-	if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]){
-		fpga->moveSortingServo(Haz);
-		fpga->moveStoppingServo(Go);
-		while(fpga->checkForBlock()) {}
-		fpga->moveStoppingServo(Stop);
-		fpga->moveSortingServo(NonHaz);
-	}else {
-		fpga->moveStoppingServo(Go);
-		while(fpga->checkForBlock()) {}
-		fpga->moveStoppingServo(Stop);
-	}
+void sortBlock(int colourValues[])
+{
+    bool isHazColour[4] = {false, false, false, false };
+
+    for (int i = 0; i < 4; i++) {
+        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
+            isHazColour[i] = true;
+        }
+    }
+
+    fpga->moveSortingServo(Haz);
+    fpga->moveStoppingServo(Go);
+    while(fpga->checkForBlock()) {}
+    fpga->moveStoppingServo(Stop);
+    wait(0.5);
+    fpga->moveSortingServo(NonHaz);
+    return;
+
+    bool isHazBlock = false;
+
+    if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]) {
+        fpga->moveSortingServo(Haz);
+        fpga->moveStoppingServo(Go);
+        while(fpga->checkForBlock()) {}
+        fpga->moveStoppingServo(Stop);
+        fpga->moveSortingServo(NonHaz);
+    } else {
+        fpga->moveStoppingServo(Go);
+        while(fpga->checkForBlock()) {}
+        fpga->moveStoppingServo(Stop);
+    }
 }
 
 void printPCDetectedText()
@@ -303,7 +289,7 @@
     LCDFL();
     lcd->printf("Abort?");
     LCDSL();
-    lcd->printf("1:YES, 2,3,4:NO");
+    lcd->printf("1:Yes, 2,3,4:No");
     int reply = 0;
     do {
         reply = readSwitches();
@@ -311,13 +297,16 @@
 
     D_LEDS_OFF();
     if (reply == 1) {
+        while (i2cport->read_bit(8)) {  }
         return true;
     } else {
+        while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) {  }
         return false;
     }
 }
 
-void printServoInfoOnLCD(){
+void printServoInfoOnLCD()
+{
     lcd->cls();
     lcd->locate(0,0);
     lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
@@ -325,28 +314,29 @@
     lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
 }
 
-void runInServoTestMode(){
-	pc.printf("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", Stop, Go, Haz, NonHaz);
-	printServoInfoOnLCD();
-	int finished = false;
-	
-	i2cport->write_bit(1, 12);
-	i2cport->write_bit(1, 13);
-	i2cport->write_bit(1, 15);
-	int button = 0;
+void runInServoTestMode()
+{
+    pc.printf("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", Stop, Go, Haz, NonHaz);
+    printServoInfoOnLCD();
+    int finished = false;
+
+    i2cport->write_bit(1, 12);
+    i2cport->write_bit(1, 13);
+    i2cport->write_bit(1, 15);
+    int button = 0;
     do {
-    	button = readSwitches();
-    	if (button == 1){
-    		fpga->toggleStoppingServo();
-    		printServoInfoOnLCD();
-    		wait(0.5);
-    	}else if (button == 2) {
-    		fpga->toggleSortingServo();
-    		printServoInfoOnLCD();
-    		wait(0.5);
-    	}
-    	finished = button == 4;
-    	button = 0;
+        button = readSwitches();
+        if (button == 1) {
+            fpga->toggleStoppingServo();
+            printServoInfoOnLCD();
+            wait(0.5);
+        } else if (button == 2) {
+            fpga->toggleSortingServo();
+            printServoInfoOnLCD();
+            wait(0.5);
+        }
+        finished = button == 4;
+        button = 0;
     } while (finished == false);
     runServoTest = false;
     D_LEDS_OFF();
@@ -357,3 +347,26 @@
     lcd->cls();
     return;
 }
+
+void displayWaitingLine()
+{
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("Waiting...");
+    lcd->locate(1,0);
+}
+
+void displayPCStatus()
+{
+    lcd->cls();
+    lcd->locate(0,0);
+    if (connectedToPC) {
+        lcd->printf("Connected to PC");
+    } else
+        lcd->printf("Waiting for PC..");
+
+    lcd->locate(1,0);
+    lcd->printf("4:Disconnect");
+    D_LEDS_OFF();
+    i2cport->write_bit(1,15);
+}
\ No newline at end of file