tsting command

src/C++/Driver/src/test.cpp

@@ -8,12 +8,14 @@ using namespace std;
 CKobuki robot;
 int command();
 
+const int forward = 1;
+const int ROTATE = 2;
+
 int main()
 {
 	unsigned char *null_ptr(0);
 	robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
 	usleep(1 * 1000 * 1000);
-	cout << "Enter commando: ";
 	thread mv(command);
 	usleep(30 * 1000 * 1000);
 	mv.join(); //only exit once thread one is done running
@@ -23,38 +25,39 @@ int checkCenterCliff()
 {
 	while (true)
 	{
-		std::cout << "cliffsensordata:" << robot.data.CliffSensorCenter << std::endl;
+		std::cout << "cliffsensordata:" << robot.parser.data.CliffSensorCenter << std::endl;
 	}
 }
 
 int checkWheelDrop(){
 	while (true)
 	{
-		std::cout << "wheeldropdata:" << robot.data.WheelDropLeft << std::endl;
+		std::cout << "wheeldropdata:" << robot.parser.data.WheelDropLeft << std::endl;
 	}
 }
 
 int command(){
-	int input;
+	cout << "choose between forward and rotate" << endl;
-
+	cout << "What must the robot do?";
 	cin >> input;
 
 	switch(input){
-		case 1:
+		case forward:{
-			robot.goStraight(1);
+			int distance;
-			robot.doRotation(90);
+			std::cout >> "Enter distance to move forward: ";
-			robot.forward(1024, -1);
+			std::cin >> distance;
+			robot.goStraight(distance);
+		}
+			
+		case ROTATE:{
+			int angle;
+			std::cout >> "Enter angle to rotate: ";
+			std::cin >> angle;
+			robot.doRotation(angle);
+		}
 			robot.goStraight(-1);
 			break;
-		case 2:
+			
-			robot.doRotation(90);
-			break;
-		case 3:
-			robot.doRotation(-90);
-			break;
-		case 4:
-			robot.forward(1024, -1);
-			break;
 		default:
 			cout << "Invalid input" << endl;
 			break;