robofsm: Competition

Different strategy implemented.

The bottle is now pushed using backward.

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index 2c8ab1c0fd57ba49cc59ea7f36d8860643a476c0..943aa577cd63d8ce63e5550d594271d99626be51 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -39,22 +39,15 @@ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
 FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
+/* from homologation */
 /* movement states - buillon */
 FSM_STATE_DECL(aproach_buillon);
-FSM_STATE_DECL(place_buillon);
-FSM_STATE_DECL(leave_buillon);
 /* Pushing the bottle */
 FSM_STATE_DECL(push_bottle);
-FSM_STATE_DECL(leave_bottle);
-FSM_STATE_DECL(goto_totem);
-FSM_STATE_DECL(approach_totem);
-FSM_STATE_DECL(leave_totem);
-
-FSM_STATE_DECL(goto_totem2);
-FSM_STATE_DECL(approach_totem2);
-FSM_STATE_DECL(leave_totem2);
-
-FSM_STATE_DECL(go_home);
+/* New states */
+FSM_STATE_DECL(reach_central_buillon);
+FSM_STATE_DECL(leave_central_buillon);
+FSM_STATE_DECL(push_bottle_bw);
 
 FSM_STATE(init)
 {
@@ -145,54 +138,19 @@ FSM_STATE(aproach_buillon)
                case EV_ENTRY:
                        robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_point_trans(
-                               0.62,
+                               0.65,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+                       robot_trajectory_add_point_trans(
+                               0.65,
+                               1.3);
                        robot_trajectory_add_final_point_trans(
-                               0.72,
-                               1.14,
-                               TURN_CW(DEG2RAD(180)));
-                       break;
-               case EV_MOTION_DONE:
-               case EV_TIMER:
-                       FSM_TRANSITION(place_buillon);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(place_buillon)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow);
-                       robot_trajectory_add_final_point_trans(
-                               ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M,
-                               1.14,
-                               NO_TURN());
-                       break;
-               case EV_MOTION_DONE:
-               case EV_TIMER:
-                       FSM_TRANSITION(leave_buillon);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(leave_buillon)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               0.64,
-                               1.14,
+                               0.5,
+                               1.1,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
                case EV_TIMER:
-                       FSM_TRANSITION(push_bottle);
+                       FSM_TRANSITION(reach_central_buillon);
                        break;
                default:
                        break;
@@ -206,168 +164,81 @@ FSM_STATE(push_bottle)
                        robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_point_trans(
                                0.64,
-                               0.9);
+                               0.7);
                        robot_trajectory_add_final_point_trans(
-                               0.64,
+                               0.64 + 0.08,
                                ROBOT_AXIS_TO_FRONT_M + 0.05,
                                ARRIVE_FROM(DEG2RAD(270), 0.10));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(leave_bottle);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(leave_bottle)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               0.64,
-                               0.4,
-                               TURN_CCW(DEG2RAD(180)));
-                       break;
-               case EV_MOTION_DONE:
-               case EV_TIMER:
-                       FSM_TRANSITION(goto_totem);
-                       break;
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(goto_totem)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               1.1,
-                               0.4,
-                               TURN_CCW(DEG2RAD(90)));
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TRANSITION(approach_totem);
+                       FSM_TRANSITION(reach_central_buillon);
                        break;
                default:
                        break;
        }
 }
 
-FSM_STATE(approach_totem)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               1.1,
-                               0.875 - ROBOT_AXIS_TO_FRONT_M - 0.05,
-                               ARRIVE_FROM(DEG2RAD(90), 0.10));
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TRANSITION(leave_totem);
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(leave_totem)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new_backward(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               1.1,
-                               0.4,
-                               NO_TURN());
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TRANSITION(goto_totem2);
-               default:
-                       break;
-       }
-}
-
-FSM_STATE(goto_totem2)
+FSM_STATE(reach_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_point_trans(
                                0.64,
-                               0.6);
+                               0.7);
                        robot_trajectory_add_point_trans(
-                               0.64,
-                               1.3);
+                               1.0,
+                               0.45);
                        robot_trajectory_add_final_point_trans(
-                               1.1,
-                               1.6,
-                               ARRIVE_FROM(DEG2RAD(270),0.1));
+                               1.3,
+                               0.58,
+                               NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(approach_totem2);
-               default:
+                       FSM_TRANSITION(leave_central_buillon);
                        break;
-       }
-}
-
-
-FSM_STATE(approach_totem2)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
-                       robot_trajectory_add_final_point_trans(
-                               1.1,
-                               1.125 + ROBOT_AXIS_TO_FRONT_M + 0.05,
-                               ARRIVE_FROM(DEG2RAD(270), 0.10));
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TRANSITION(leave_totem2);
                default:
                        break;
        }
 }
 
-FSM_STATE(leave_totem2)
+FSM_STATE(leave_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new_backward(&tcSlow); // new trajectory
                        robot_trajectory_add_final_point_trans(
-                               1.1,
-                               1.6,
+                               1.0,
+                               0.45,
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(go_home);
+                       FSM_TRANSITION(push_bottle_bw);
+                       break;
                default:
                        break;
        }
 }
 
-FSM_STATE(go_home)
+FSM_STATE(push_bottle_bw)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_point_trans(
+                               0.7,
+                               0.3);
                        robot_trajectory_add_final_point_trans(
-                               0.5 + ROBOT_AXIS_TO_FRONT_M,
-                               1.75,
-                               ARRIVE_FROM(DEG2RAD(180),0.1));
+                               0.64+0.08,
+                               ROBOT_AXIS_TO_BACK_M + 0.05,
+                               ARRIVE_FROM(DEG2RAD(270), 0.10));
                        break;
                case EV_MOTION_DONE:
+                       break;
                default:
                        break;
        }
 }
-
-// totem 1100 x 1000 polovička totemu 125
-// druhá láhev na 1883
-
 /************************************************************************
  * MAIN FUNCTION
  ************************************************************************/