Diff: main.cpp

Revision:

26:bbcc25418ffa

Parent:

25:792540d69c49

Child:

27:2cb1bdb7ae3d

diff -r 792540d69c49 -r bbcc25418ffa main.cpp
--- a/main.cpp	Mon Nov 30 15:06:38 2015 +0000
+++ b/main.cpp	Mon Nov 30 21:29:51 2015 +0000
@@ -72,18 +72,21 @@
 	// https://developer.mbed.org/cookbook/Serial-Interrupts
 	pc.attach(&Rx_interrupt, Serial::RxIrq);
 
+	DefaultHazBlock();
+			fpga->moveStoppingServo(Stop);
+		fpga->moveSortingServo(NonHaz);
+				pc.printf(":<pc>connect;");
+	
 	for (;;) {
+		if (connectedToPC == false){
 		lcd->cls();
 		i2cport->write_bit(1, 12);
 		lcd->printf("1: Start sorting.");
 		LCDSL();
 		i2cport->write_bit(1,13);
 		lcd->printf("2: Connect to PC");
-
-		fpga->moveStoppingServo(Stop);
-		fpga->moveSortingServo(NonHaz);
+		}
 		
-		pc.printf(":<pc>connect;");
 //		wait(0.03);
 
 		int selection = 0;
@@ -146,6 +149,14 @@
 							runInBreakBeamTestMode();
 						else if (runColourSensorTest == true)
 							runInColourSensorTestMode();
+						if (i2cport->read_bit(11) == 1){
+							if (displayAbortDialog() == true){
+								currentMode = None;
+								pc.printf(":<mbed>mode=none;");
+							}else {
+								displayPCStatus();
+							}
+						}
 						}
 					} else if (currentMode == Normal) {
 						displayPCStatus();
@@ -162,10 +173,11 @@
 								while (currentState == Pause && currentMode == Normal) {
 									button = readSwitches();
 									if (button == 1) {
-										pc.printf("state:start\n");
+										pc.printf(":<mbed>sort=start;");
 										currentState = Start;
 									} else if (button == 4) {
 										currentMode = None;
+										pc.printf(":<mbed>mode=none;");
 										//                                        goto setModeNone;
 										break;
 									}
@@ -177,9 +189,9 @@
 								lcd->printf("Sorting mode...");
 								while (currentState == Start && currentMode == Normal) {
 									if (waitForBlock() == false) {
-										if (connectedToPC == true) {
+										if (connectedToPC == true && currentState != Pause) {
 											// TODO: Tell PC to update UI if aborted from MBED.
-											pc.printf("state:pause\n");
+											pc.printf(":<mbed>sort=pause;");
 											currentState = Pause;
 											continue;
 										} else if (connectedToPC == false) {
@@ -272,6 +284,7 @@
 		else
 			return true;
 	}
+	return false;
 }
 
 void sortBlock()
@@ -322,7 +335,7 @@
 		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("DEBUG:%i Pass.\n", i);
 		}
 		pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], haz, percentageError[0], percentageError[1], percentageError[2]);
 		//        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], haz);
@@ -387,6 +400,7 @@
 	memset(adjustedValues, 0, sizeof(adjustedValues));
 	memset(percentageError, 0, sizeof(percentageError));
 
+/* Harcoded working
 	for (int i = 0; i < 3; i++) {
 		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
 		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
@@ -395,7 +409,16 @@
 			isHazColour[i] = true;
 		}
 	}
+*/
+	for (int i = 0; i < 3; i++) {
+		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
+		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
 
+		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
+			isHazColour[i] = true;
+		}
+	}
+	
 	//    for (int i = 0; i < 3; i++) {
 		//        if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]) {
 	//            isHazColour[i] = true;
@@ -491,12 +514,12 @@
 		button = readSwitches();
 		
 		// gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
-		if (gToggleSeroNumber == 1){
+		if (gToggleServoNumber == 1){
 			button = 1;
 			gToggleServoNumber = 0;
 		}else if (gToggleServoNumber == 2){
 			button = 2;
-			gTogglerServoNumber = 0;
+			gToggleServoNumber = 0;
 		}
 		
 		if (button == 1) {
@@ -590,6 +613,7 @@
 	for (int i = 0; i < 3; i++){
 		weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
 	}
+	//TODO: Print values on LCD
 	lcd->cls();
 }