robofsm: Competition strategy

Finished first strategy for competition.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index d0c3b1c1f226cd6fca0a66144c893f1971046a68..ad31ad135076be0b7cb26af34a7cd8abc49717cd 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -104,7 +104,9 @@ void inline enable_bumpers(bool enabled)
  ************************************************************************/
 
 struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-
+double totem_x;
+double totem_y;
+bool up;
 
 FSM_STATE(approach_central_buillon)
 {
@@ -112,15 +114,18 @@ FSM_STATE(approach_central_buillon)
                case EV_ENTRY:
                        robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_point_trans(
-                               0.55,
+                               0.5,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+                       robot_trajectory_add_point_trans(
+                               0.64,
+                               1.2);
                        robot_trajectory_add_point_trans(
                                0.64,
                                0.7);
                        robot_trajectory_add_final_point_trans(
                                1.0,
                                0.45,
-                               NO_TURN());
+                               ARRIVE_FROM(DEG2RAD(24),0.1));
 //                     robot_trajectory_add_final_point_trans(
 //                             1.3,
 //                             0.58,
@@ -164,8 +169,8 @@ FSM_STATE(leave_central_buillon)
                case EV_ENTRY:
                        robot_trajectory_new_backward(&tcSlow); // new trajectory
                        robot_trajectory_add_final_point_trans(
-                               1.0,
-                               0.45,
+                               0.85,
+                               0.38,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
@@ -212,10 +217,11 @@ FSM_STATE(return_home)
                                ARRIVE_FROM(DEG2RAD(180), 0.10));
                        break;
                case EV_MOTION_DONE:
+                       enable_bumpers(true);
                        FSM_TIMER(2000);
                        break;
                case EV_TIMER:
-                       FSM_TRANSITION(leave_home);
+                       SUBFSM_RET(NULL);
                        break;
                default:
                        break;
@@ -230,15 +236,189 @@ FSM_STATE(leave_home)
                        robot_trajectory_add_final_point_trans(
                                0.6,
                                1.0,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(leave_home_for_totem);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_home_for_totem)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       if(up) {
+                               robot_trajectory_add_final_point_trans(
+                                       0.64,
+                                       1.3,
+                                       NO_TURN());
+                       }
+                       else {
+                               robot_trajectory_add_final_point_trans(
+                                       0.64,
+                                       0.7,
+                                       NO_TURN());
+                       }
+                       break;
+               case EV_MOTION_DONE:
+                       if(up) FSM_TRANSITION(approach_totem_up);
+                       else FSM_TRANSITION(approach_totem_down); 
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(approach_totem_down)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x,
+                               0.48,
                                TURN_CCW(DEG2RAD(90)));
                        break;
                case EV_MOTION_DONE:
                        FSM_TIMER(2000);
                        break;
+               case EV_TIMER:
+                       FSM_TRANSITION(catch_totem_buillon_down);
+                       break;
+               default:
+                       break;
+       }
+}
+ 
+FSM_STATE(catch_totem_buillon_down)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x, 
+                               totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
+                               ARRIVE_FROM(DEG2RAD(90), 0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TIMER(2000);
+                       break;
+               case EV_TIMER:
+                       FSM_TRANSITION(leave_totem_down);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_totem_down)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x,
+                               0.48,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(place_buillon_home);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(place_buillon_home)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       if(up) {
+                               robot_trajectory_add_point_trans(
+                                       0.9,
+                                       1.52);
+                       }
+                       else {
+                               robot_trajectory_add_point_trans(
+                                       0.9,
+                                       0.48);
+                               robot_trajectory_add_point_trans(
+                                       0.7,
+                                       0.8);
+                       }
+                       robot_trajectory_add_final_point_trans(
+                               0.4,
+                               1.0,
+                               ARRIVE_FROM(DEG2RAD(180),0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TIMER(2000);
+                       break;
                case EV_TIMER:
                        SUBFSM_RET(NULL);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(approach_totem_up)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x,
+                               1.52,
+                               TURN_CW(DEG2RAD(270)));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TIMER(2000);
+                       break;
+               case EV_TIMER:
+                       FSM_TRANSITION(catch_totem_buillon_up);
                        break;
                default:
                        break;
        }
+}
+
+FSM_STATE(catch_totem_buillon_up)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x,
+                               totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
+                               ARRIVE_FROM(DEG2RAD(270), 0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TIMER(2000);
+                       break;
+               case EV_TIMER:
+                       FSM_TRANSITION(leave_totem_up);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_totem_up)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               totem_x,
+                               1.52,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       SUBFSM_RET(NULL);
+               default:
+                       break;
+       }
 }
\ No newline at end of file

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 8be60eb57847d8b4278cc786dfacbbefbcc1ce19..e17520f0e0c75dfa201fed612f51fb8bde35ee97 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -7,7 +7,8 @@
 #include <fsm.h>
 
 extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-
+extern double totem_x, totem_y;
+extern bool up;
 /* strategy FSM */
 FSM_STATE_DECL(get_central_buillon_first);
 FSM_STATE_DECL(ignore_central_buillon);
@@ -19,17 +20,23 @@ FSM_STATE_DECL(catch_central_buillon);
 FSM_STATE_DECL(leave_central_buillon);
 FSM_STATE_DECL(push_bottle_bw);
 FSM_STATE_DECL(return_home);
-FSM_STATE_DECL(leave_home);
 
 /* Ignore central buillon */
 //FSM_STATE_DECL(push_nearest_buillon);
 //FSM_STATE_DECL(push_bottle_fw);
 
 /* Common states */
-/*FSM_STATE_DECL(approach_totem_down);
-FSM_STATE_DECL(catch_down_totem_buillon);
+FSM_STATE_DECL(leave_home);
+FSM_STATE_DECL(leave_home_for_totem);
+FSM_STATE_DECL(approach_totem_down);
+FSM_STATE_DECL(catch_totem_buillon_down);
 FSM_STATE_DECL(leave_totem_down);
-FSM_STATE_DECL(place_down_buillon);
+FSM_STATE_DECL(place_buillon_home);
+FSM_STATE_DECL(approach_totem_up);
+FSM_STATE_DECL(catch_totem_buillon_up);
+FSM_STATE_DECL(leave_totem_up);
+
+/*FSM_STATE_DECL(place_down_buillon);
 FSM_STATE_DECL(approach_totem_up);
 FSM_STATE_DECL(catch_up_totem_buillon);
 FSM_STATE_DECL(leave_totem_up);

diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc
index 5ea9b2cc67111db8b15ccf3a94e363152408f2f5..f380f94b3e80df41cfafd6d0e47a671a331674c6 100644 (file)
--- a/src/robofsm/strategy_get_central_buillon.cc
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -38,6 +38,9 @@ FSM_STATE(get_central_buillon_first)
 #define FSM_STATE_VISIBILITY static
 
 FSM_STATE_DECL(bottle_bw);
+FSM_STATE_DECL(pick_buillon_from_totem1);
+FSM_STATE_DECL(pick_buillon_from_totem2);
+
 
 FSM_STATE(pick_central_buillon)
 {
@@ -48,7 +51,8 @@ FSM_STATE(pick_central_buillon)
                case EV_RETURN:
                        FSM_TRANSITION(bottle_bw);
                        break;
-               default:;
+               default:
+                       break;
        }
 }
 
@@ -57,9 +61,42 @@ FSM_STATE(bottle_bw)
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        SUBFSM_TRANSITION(push_bottle_bw,NULL);
+                       up = false;
                        break;
                case EV_RETURN:
-                       //FSM_TRANSITION(approach_totem_dowm)
-               default:;
+                       FSM_TRANSITION(pick_buillon_from_totem1);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(pick_buillon_from_totem1)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       totem_x = 1.1;
+                       totem_y = 0.875;
+                       SUBFSM_TRANSITION(leave_home,NULL);
+                       break;
+               case EV_RETURN:
+                       FSM_TRANSITION(pick_buillon_from_totem2);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(pick_buillon_from_totem2)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       up = true;
+                       totem_x = 1.1;
+                       totem_y = 1.125;
+                       SUBFSM_TRANSITION(leave_home,NULL);
+                       break;
+               default:
+                       break;
        }
 }
\ No newline at end of file