Diff: commander.cpp

Revision:

25:792540d69c49

Parent:

23:db91aaa43a9e

Child:

27:2cb1bdb7ae3d

--- a/commander.cpp	Sun Nov 29 01:05:42 2015 +0000
+++ b/commander.cpp	Mon Nov 30 15:06:38 2015 +0000
@@ -16,8 +16,8 @@
 string CommandObjectCommandsValue [6][kMaxCommandCount] = {
 		{"mbed", "haz-block", "read-current-block", "mode", "sort", "", "", "", "", ""},
 		{"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
-		{"colour_sensor", "i-time", "preview", "", "", "", "", "", "", ""},
-		{"servos", "test", "reset", "stop-test", "", "", "", "", "", ""},
+		{"colour_sensor", "i-time", "preview", "value", "block-value", "test", "", "", "", ""},
+		{"servos", "test", "reset", "toggle", "", "", "", "", "", ""},
 		{"port", "init", "b-rate", "parity", "", "", "", "", "", ""},
 		{"break_beam", "test", "", "", "", "", "", "", "", ""},
 };
@@ -63,18 +63,19 @@
 	return;
 }
 
+//TODO: Fix format
 std::string Commander::description()
 {
 	string str;
-	str.append("Command type:\t");
+	str.append("Type:\t\t");
 	str.append(&this->typeChar);
-	str.append("\nCommand object:\t" + this->object);
+	str.append("\nObject:\t\t" + this->object);
 
 	for (int i = 0; i < sizeof(this->command)/sizeof(*this->command); i++) {
 		if (this->command[i] == "") {
 			break;
 		}
-		str.append("\nCommand:\t" + this->command[i] + "\nValue:\t" + this->commandValue[i].c_str() + " \n");
+		str.append("\nCommand:\t" + this->command[i] + "\nValue:\t\t" + this->commandValue[i].c_str() + " \n");
 	}
 	return str;
 }
@@ -258,8 +259,16 @@
 				} else if (this->commandValue[i] == "OFF") {
 					previewOnPC(false);
 				}
-			} else if (this->commandIndex[i] == 3) {
-				readColourSensor();
+			} else if (this->commandIndex[i] == 3 && runColourSensorTest == true && getColourSensorValue == false) {
+				getColourSensorValue = true;
+			} else if (this->commandIndex[i] == 4 && runColourSensorTest == true && getBlockColourValue == false){
+				getBlockColourValue = true;	
+			} else if (this->commandIndex[i] == 5){
+				if (this->commandValue[i] == "start") {
+					testColourSensor(Start);
+				} else if (this->commandValue[i] == "pause") {
+					testColourSensor(Pause);
+				}
 			}
 		}
 		break;
@@ -277,7 +286,18 @@
 			} else if (this->commandIndex[i] == 2) {
 				resetServos();
 			} else if (this->commandIndex[i] == 3) {
-
+				if (this->commandValue[i] == "1"){
+					pc.printf("INFO: Moving top servo.\n");
+					gToggleServoNumber = 1;
+				}
+				else if (this->commandValue[i] == "2"){
+					pc.printf("INFO: Moving bottom servo.\n");
+					gToggleServoNumber = 2;
+				}
+				else if (this->commandValue[i] == "3"){
+					pc.printf("INFO: Moving both servos.\n");
+					gToggleServoNumber = 3;
+				}
 			}
 		}
 		break;