Diff: main.cpp

Revision:

25:792540d69c49

Parent:

24:02c61793f90b

Child:

26:bbcc25418ffa

--- a/main.cpp	Sun Nov 29 01:05:42 2015 +0000
+++ b/main.cpp	Mon Nov 30 15:06:38 2015 +0000
@@ -55,6 +55,7 @@
 void runInBreakBeamTestMode();
 void turnOffTopLEDs();
 void turnOffBottomLEDs();
+void runInColourSensorTestMode();
 
 int main()
 {
@@ -65,7 +66,7 @@
 	lcd->cls();
 
 	rgbSensor.enablePowerAndRGBC();
-	rgbSensor.setIntegrationTime(3);
+	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
@@ -143,6 +144,8 @@
 							runInServoTestMode();
 						else if (runBreakBeamTest == true)
 							runInBreakBeamTestMode();
+						else if (runColourSensorTest == true)
+							runInColourSensorTestMode();
 						}
 					} else if (currentMode == Normal) {
 						displayPCStatus();
@@ -450,15 +453,15 @@
 	lcd->cls();
 	lcd->locate(0,0);
 	if (fpga->stoppingServoPosition == Stop)
-		lcd->printf("1: Top: Stop");
+		lcd->printf("1:Top: Go");
 	else if (fpga->stoppingServoPosition == Go)
-		lcd->printf("1: Top: Go");
+		lcd->printf("1:Top: Stop");
 
 	lcd->locate(1,0);
 	if (fpga->sortingServoPosition == NonHaz)
-		lcd->printf("2: Bottom: NonHaz");
+		lcd->printf("2:Bottom: Haz");
 	else if (fpga->sortingServoPosition == Haz)
-		lcd->printf("2: Bottom: Haz");
+		lcd->printf("2:Bottom: NonHaz");
 }
 
 void printServoInfoOnPC() {
@@ -486,6 +489,16 @@
 	bool finished = false;
 	do {
 		button = readSwitches();
+		
+		// gToggleServoNumber: 1 = Toggle top servo, 2 = Toggle bottom servo, 3 = Toggle both servos
+		if (gToggleSeroNumber == 1){
+			button = 1;
+			gToggleServoNumber = 0;
+		}else if (gToggleServoNumber == 2){
+			button = 2;
+			gTogglerServoNumber = 0;
+		}
+		
 		if (button == 1) {
 			fpga->toggleStoppingServo();
 			printServoInfoOnLCD();
@@ -497,6 +510,7 @@
 			printServoInfoOnPC();
 			wait(kServoWait);
 		}
+			
 		finished = button == 4;
 		button = 0;
 	} while (finished == false && runServoTest == true);
@@ -523,7 +537,7 @@
 }
 
 void printBeamInfoOnPC(int topBeam, int bottomBeam){
-	pc.printf(":<break_beam>1=%i,2=%i;", topBeam, bottomBeam);
+	pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
 }
 
 void runInBreakBeamTestMode(){
@@ -570,6 +584,91 @@
 	return;
 }
 
+void printColourSensorInfoOnLCD(int colourValues[]){
+	float weightedValues[4];
+	
+	for (int i = 0; i < 3; i++){
+		weightedValues[i] = (float)colourValues[i] / (float)colourValues[3];
+	}
+	lcd->cls();
+}
+
+void printColourSensorInfoOnPC(int colourValues[]){
+	pc.printf("!<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
+}
+
+void runInColourSensorTestMode() {
+	turnOffTopLEDs();
+	i2cport->write_bit(1, 15);
+	i2cport->write_bit(1, 12);
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Colour Sensor");
+	lcd->locate(1,0);
+	lcd->printf("Test Mode");
+	pc.printf("!<colour_sensor>i-time=%.3f", gIntegrationTime);
+	
+	int button = 0;
+	bool finished = false;
+	
+	do {
+		button = readSwitches();
+		
+		if (getColourSensorValue == true){
+			int colourValues[4];
+			rgbSensor.getAllColors(colourValues);
+			printColourSensorInfoOnPC(colourValues);
+			printColourSensorInfoOnLCD(colourValues);
+			getColourSensorValue = false;
+		} else if (getBlockColourValue == true){
+			int topBeam = fpga->getBeamValue(1);
+			
+			while(topBeam == 0){
+				topBeam = fpga->getBeamValue(1);
+			}
+			int colourValues[3][4];
+			for (int i = 0; i < 3; i++) { memset(colourValues[i], 0, sizeof(colourValues)); }
+			int valueCount = 0;
+			do {
+				rgbSensor.getAllColors(colourValues[valueCount]);
+			} while (topBeam == 1 && valueCount < 3);
+			int averageValues[4] = {0, 0, 0, 0};
+			for (int i = 0; i < 3; i++) {
+				averageValues[0] += colourValues[i][0];
+				averageValues[1] += colourValues[i][1];
+				averageValues[2] += colourValues[i][2];
+				averageValues[3] += colourValues[i][3];
+			}
+			averageValues[0] = averageValues[0] / valueCount;
+			averageValues[1] = averageValues[1] / valueCount;
+			averageValues[2] = averageValues[2] / valueCount;
+			averageValues[3] = averageValues[3] / valueCount;
+			
+			printColourSensorInfoOnPC(averageValues);
+			printColourSensorInfoOnLCD(averageValues);
+			
+			getBlockColourValue = false;
+		} else if (button == 1){
+			getColourSensorValue = true;
+			button = 0;
+		}
+		
+		finished = button == 4;
+//		button = 0;
+	} while (finished == false && runColourSensorTest == true);
+	
+	turnOffBottomLEDs();
+	lcd->cls();
+	lcd->locate(0,0);
+	lcd->printf("Finished test");
+	if (runColourSensorTest == true){
+		pc.printf(":<colour_sensor>test=pause;");
+		runColourSensorTest = false;
+	}
+	wait(0.5);
+	return;
+}
+
 void displayWaitingLine()
 {
 	lcd->cls();