robofsm: approaching opponent's static dispenser

diff --git a/src/robodim/robodim.h b/src/robodim/robodim.h
index d7751327fa9d662d4bcb8a94090f5ff3877e91dc..ccd05c79e1f9b491a09078d3d916346f9119c051 100644 (file)
--- a/src/robodim/robodim.h
+++ b/src/robodim/robodim.h
@@ -87,6 +87,8 @@
 #define STATIC_DISPENSER_Y             DISPENSER_TO_CUSHION_DISTANCE
 #define TRAVELLING_DISPENSER_X         (PLAYGROUND_WIDTH - DISPENSER_TO_CUSHION_DISTANCE)
 // y coordinate of the travelling dispenser is not known in advance
+#define OPPONENTS_STATIC_DISPENSER_X   STATIC_DISPENSER_WIDTH_OFFSET
+#define OPPONENTS_STATIC_DISPENSER_Y   DISPENSER_TO_CUSHION_DISTANCE
 
 /**
  * FREE PUCKS POSITIONS

diff --git a/src/robofsm/fsmmain.c b/src/robofsm/fsmmain.c
index 88ed6c67f45e102a75949310659ceae7814e2491..d6977ef8d9f35e2eecdfc6892abbb877edfa9754 100644 (file)
--- a/src/robofsm/fsmmain.c
+++ b/src/robofsm/fsmmain.c
@@ -134,6 +134,7 @@ FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
 /* movement states */
 FSM_STATE_DECL(approach_our_static_dispenser);
+FSM_STATE_DECL(approach_opponents_static_dispenser);
 
 /************************************************************************
  * INITIAL AND STARTING STATES
@@ -170,12 +171,13 @@ FSM_STATE(wait_for_start)
 #endif
                        robot_set_est_pos_trans(ROBOT_WIDTH_M,
                                                PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M - 0.1,
-                                               DEG2RAD(-90));
+                                               DEG2RAD(-45));
                        /* start to do something */
                        ROBOT_LOCK(state);
                        robot.state = COMPETITION_STARTED;
                        ROBOT_UNLOCK(state);
-                       FSM_TRANSITION(approach_our_static_dispenser);
+                       //FSM_TRANSITION(approach_our_static_dispenser);
+                       FSM_TRANSITION(approach_opponents_static_dispenser);
                        break;
                case EV_RETURN:
                case EV_TIMER:
@@ -237,3 +239,41 @@ FSM_STATE(approach_our_static_dispenser)
        }
 }
 
+FSM_STATE(approach_opponents_static_dispenser)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:  {
+                       struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+                       tc.maxv /= 1.0;
+                       robot_trajectory_new(&tc);
+
+                       robot_trajectory_add_point_trans(OPPONENTS_STATIC_DISPENSER_X,
+                                                        OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M + 0.4);
+                       robot_trajectory_add_final_point_trans(OPPONENTS_STATIC_DISPENSER_X,
+                                                              OPPONENTS_STATIC_DISPENSER_Y + ROBOT_AXIS_TO_FRONT_M+0.15,
+                                                              NO_TURN());
+                       }
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_SIGNAL(ACT, EV_PICK_UP_PUCK, NULL);
+                       printf("Arrived to the static dispenser\n");
+                       break;
+               case EV_PUCK_LOADED_UP:
+                       printf("A Puck picked up\n");
+                       break;
+               case EV_RETURN:
+               case EV_TIMER:
+               case EV_OBSTRUCTION_AHEAD:
+               case EV_LASER_POWER:
+               case EV_GOAL_NOT_REACHABLE:
+               case EV_SHORT_TIME_TO_END:
+               case EV_ENEMY_AHEAD:
+               case EV_STACK_FULL:
+               case EV_START:
+                       DBG_PRINT_EVENT("unhandled event");
+                       break;
+               case EV_EXIT:
+                       break;
+       }
+}
+