From cc830215ad18e0ae84bf7b20ef34b66c77d03c28 Mon Sep 17 00:00:00 2001
From: Matous Pokorny <matous.pokorny@me.com>
Date: Mon, 2 Jul 2012 16:54:52 +0200
Subject: [PATCH] pathplan/test: Add slower speed and other start and goal
 position

---
 src/robofsm/test/goto.cc | 55 +++++++++++++++++++++++++++++++++++-----
 1 file changed, 48 insertions(+), 7 deletions(-)

diff --git a/src/robofsm/test/goto.cc b/src/robofsm/test/goto.cc
index 904d9b9f..4b605ecf 100644
--- a/src/robofsm/test/goto.cc
+++ b/src/robofsm/test/goto.cc
@@ -31,9 +31,13 @@ FSM_STATE_DECL(robot_goto_test);
 void set_initial_position()
 {
 	//FIXME:
-	robot_set_est_pos_trans(0.5 - ROBOT_AXIS_TO_BACK_M,
-				PLAYGROUND_HEIGHT_M - 0.5 + (ROBOT_WIDTH_M/2),
-				DEG2RAD(180));
+// 	robot_set_est_pos_trans(0.5 - ROBOT_AXIS_TO_BACK_M,
+// 				PLAYGROUND_HEIGHT_M - 0.5 + (ROBOT_WIDTH_M/2),
+// 				DEG2RAD(180));
+	robot_set_est_pos_trans(
+		PLAYGROUND_WIDTH_M/2,
+		PLAYGROUND_HEIGHT_M/2,
+		DEG2RAD(0));
 }
 
 FSM_STATE(init)
@@ -48,18 +52,55 @@ FSM_STATE(init)
 FSM_STATE(robot_goto_test)
 {
 	double goalx, goaly;
+	
+	struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+	
+	tcFast = trajectoryConstraintsDefault;
+	tcFast.maxv = 0.8;
+	tcFast.maxacc = 1;
+	tcFast.maxomega = 2;
+	
+	tcSlow = trajectoryConstraintsDefault;
+	tcSlow.maxv = 0.35;
+	tcSlow.maxacc = 0.5;
+	tcSlow.maxe = 0.5;
+	
+	tcVerySlow = trajectoryConstraintsDefault;
+	tcVerySlow.maxv = 0.05;
+	tcVerySlow.maxacc = 0.05;
+	
+	struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+	
+	tc = tcSlow;
+	
 	switch (FSM_EVENT) {
 		case EV_ENTRY:
 			//ShmapSetRectangleFlag(1.3, 0.7, 1.7, 1.0, MAP_FLAG_SIMULATED_WALL, 0);
 			//ShmapSetRectangleFlag(1.8, 1.5, 2.3, 1.8, MAP_FLAG_SIMULATED_WALL, 0);
 			do {
-				goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM-50)+25)/1000.0;
-				goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM-500)+25)/1000.0);
+				goalx = (rand()%(MAP_PLAYGROUND_WIDTH_MM))/1000.0;
+				goaly = ((rand()%(MAP_PLAYGROUND_HEIGHT_MM))/1000.0);
+				
+				if(goalx > MAP_PLAYGROUND_WIDTH_M/2) {
+					goalx =- 0.5;
+				}
+				else {
+					goalx =+ 0.5;
+				}
+				
+				if(goaly > MAP_PLAYGROUND_HEIGHT_M/2) {
+					goaly =- 0.5;
+				}
+				else {
+					goaly =+ 0.5;
+				}
+				
 // 				goalx = robot.act_pos->x + (rand()%100)/1000.0;
 // 				goaly = robot.act_pos->y + (rand()%100)/1000.0;
 			} while (!ShmapIsFreePoint(goalx, goaly));
-
-			robot_goto_notrans(goalx, goaly, NO_TURN(), NULL);
+			
+			printf("Goal coordinates %1.2f %1.2f\n", goalx, goaly);
+			robot_goto_notrans(goalx, goaly, NO_TURN(), &tc);
 			break;
 		case EV_MOTION_ERROR:
 			ul_logdeb("Goal is not reachable\n");
-- 
2.39.2