homolagation: working betaversion

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index f8e46c62fad74f3c946773d5de08e52874b1f376..f1e7d0f77f421a0dcb1a3932f7a1aed49b55340a 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -101,6 +101,7 @@ Pos * get_corn_approach_position(struct corn *corn)
 /* initial and starting states */
 FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
+FSM_STATE_DECL(wait);
 /* movement states */
 FSM_STATE_DECL(climb_the_slope);
 FSM_STATE_DECL(sledge_down);
@@ -116,8 +117,9 @@ FSM_STATE(init)
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        tcFast = trajectoryConstraintsDefault;
-                       tcFast.maxv = 1.5;
+                       tcFast.maxv = 1;
                        tcFast.maxacc = 0.3;
+                       tcFast.maxomega = 2;
                        tcSlow = trajectoryConstraintsDefault;
                        tcSlow.maxv = 0.3;
                        tcSlow.maxacc = 0.3;
@@ -152,8 +154,7 @@ FSM_STATE(wait_for_start)
                case EV_ENTRY:
 #endif
                        //FIXME:
-                       robot_set_est_pos_trans(ROBOT_AXIS_TO_BRUSH_M,
-                                               //PLAYGROUND_HEIGHT_M - 0.355,
+                       robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
                                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
                                                DEG2RAD(180));
                        FSM_TRANSITION(climb_the_slope);
@@ -182,19 +183,18 @@ FSM_STATE(climb_the_slope)
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot.ignore_hokuyo = true;
-                       robot_trajectory_new_backward(&tcFast);
+                       robot_trajectory_new_backward(&tcSlow);
                        robot_trajectory_add_point_trans(
-                               SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M + 0.25,
-                               //PLAYGROUND_HEIGHT_M - 0.355);
+                               0.5 - ROBOT_AXIS_TO_BACK_M,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
+                       /* position for collecting oranges*/
                        robot_trajectory_add_final_point_trans(
-                               SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M + 0.5,
-                               //PLAYGROUND_HEIGHT_M - 0.4/*0.355*/,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
+                               SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.04,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.02,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(sledge_down);
+                       FSM_TRANSITION(wait);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -208,23 +208,31 @@ FSM_STATE(climb_the_slope)
        }
 }
 
+FSM_STATE(wait)
+{
+       DBG_PRINT_EVENT("Loading oranges");
+       FSM_TIMER(5000);
+       switch (FSM_EVENT) {
+               case EV_TIMER:
+                       FSM_TRANSITION(sledge_down);
+                       break;
+               default:
+                       break;
+       }
+}
+
 FSM_STATE(sledge_down)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new(&tcFast);
+                       robot_trajectory_add_point_trans(
+                               1 -ROBOT_AXIS_TO_BACK_M,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.02);
                        robot_trajectory_add_point_trans(
                                SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
-                               //PLAYGROUND_HEIGHT_M - 0.355);
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
                        robot_trajectory_add_final_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6, NO_TURN());
-
-                       /*
-                       robot_trajectory_new(&tcSlow);
-                       robot_trajectory_add_final_point_trans(
-                               SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
-                               PLAYGROUND_HEIGHT_M - 0.355,
-                               NO_TURN()); */
                        break;
                case EV_MOTION_DONE:
                        FSM_TRANSITION(to_container_diag);
@@ -248,18 +256,11 @@ FSM_STATE(to_container_diag)
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new(&tcFast);
-                       /*robot_trajectory_add_point_trans(
-                               SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26,
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
-                               //PLAYGROUND_HEIGHT_M - 0.355); */
-                       robot_trajectory_add_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6);
-
                        // face the rim with front of the robot
                        //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
                        // face the rim with back of the robot
                        robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
-                       //robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.4, 0.30);
-                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN(DEG2RAD(90)));
+                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
                        break;
                case EV_START:
                case EV_TIMER:
@@ -319,8 +320,8 @@ int main()
        robot.obstacle_avoidance_enabled = true;
 
        robot.fsm.main.debug_states = 1;
-       robot.fsm.motion.debug_states = 1;
-       robot.fsm.act.debug_states = 1;
+       //robot.fsm.motion.debug_states = 1;
+       //robot.fsm.act.debug_states = 1;
 
        robot.fsm.main.state = &fsm_state_main_init;
        //robot.fsm.main.transition_callback = trans_callback;

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index dd7e057717c3161bc070dfdd892fc33dac7aefbf..61d3f02fb339a2e1aeed3c21388c39faec643dce 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -309,8 +309,6 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        if(!robot.ignore_hokuyo)
                        {
                                update_map_hokuyo(instance);
-                       } else {
-                               printf("Hokuyo_scan_cb: scan ignored, map not updated!\n");
                        }
                        break;
                }