pathplan/test: Add slower speed and other start and goal position

[eurobot/public.git] / src / robofsm / test / goto.cc

diff --git a/src/robofsm/test/goto.cc b/src/robofsm/test/goto.cc
index 904d9b9fa0d4becf3d65e6b6ef52a5eb84b7b4fe..4b605ecf2aa4d960dbd80726f2d136ae616d8483 100644 (file)
--- 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");