]> rtime.felk.cvut.cz Git - eurobot/public.git/blobdiff - src/robofsm/competition2012.cc
robofsm: Competition2012
[eurobot/public.git] / src / robofsm / competition2012.cc
index a48f94ba85ba4eb9ad79095bdb4933e41677d3fd..9f320d090b98276f2b5cc5bdf2fcb16721a9f6ae 100644 (file)
@@ -39,17 +39,16 @@ 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(go_home);
+/* New states */
+FSM_STATE_DECL(reach_central_buillon);
+FSM_STATE_DECL(leave_central_buillon);
+FSM_STATE_DECL(push_bottle_bw);
+FSM_STATE_DECL(return_home);
 
 FSM_STATE(init)
 {
@@ -140,54 +139,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;
@@ -201,117 +165,102 @@ 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);
+                       FSM_TRANSITION(reach_central_buillon);
                        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)
+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.7);
+                       robot_trajectory_add_point_trans(
+                               1.0,
+                               0.45);
                        robot_trajectory_add_final_point_trans(
-                               1.1,
-                               0.4,
-                               TURN_CCW(DEG2RAD(90)));
+                               1.3,
+                               0.58,
+                               NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(approach_totem);
+                       FSM_TRANSITION(leave_central_buillon);
                        break;
                default:
                        break;
        }
 }
 
-FSM_STATE(approach_totem)
+FSM_STATE(leave_central_buillon)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_new_backward(&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));
+                               1.0,
+                               0.45,
+                               NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(leave_totem);
+                       FSM_TRANSITION(push_bottle_bw);
+                       break;
                default:
                        break;
        }
 }
 
-FSM_STATE(leave_totem)
+FSM_STATE(push_bottle_bw)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_point_trans(
+                               0.7,
+                               0.3);
                        robot_trajectory_add_final_point_trans(
-                               1.1,
-                               0.4,
-                               NO_TURN());
+                               0.64+0.08,
+                               ROBOT_AXIS_TO_BACK_M + 0.05,
+                               ARRIVE_FROM(DEG2RAD(270), 0.10));
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(go_home);
+                       FSM_TRANSITION(return_home);
+                       break;
                default:
                        break;
        }
 }
 
-
-FSM_STATE(go_home)
+FSM_STATE(return_home)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_point_trans(
-                               0.64,
-                               0.6);
-                       robot_trajectory_add_point_trans(
-                               0.64,
-                               1.0);
+                               0.64 + 0.08,
+                               0.7);
                        robot_trajectory_add_final_point_trans(
-                               0.35,
+                               0.4,
                                1.0,
-                               ARRIVE_FROM(DEG2RAD(180),0.1));
+                               ARRIVE_FROM(DEG2RAD(180), 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
  ************************************************************************/