robofsm: Homologation

More elegant version of homologation with no backward.

diff --git a/src/robofsm/homologation2012.cc b/src/robofsm/homologation2012.cc
index 1a15c0b6472f47eb0e2cb7b66f3ee04bde118038..3d21d28b38f8331c821bfc6ad74c7e03aea2209b 100644 (file)
--- a/src/robofsm/homologation2012.cc
+++ b/src/robofsm/homologation2012.cc
@@ -1,5 +1,5 @@
 /*
- * homologation.cc       12/02/28
+ * homologation.cc       12/03/15
  *
  * Robot's control program intended for homologation (approval phase) on Eurobot 2012.
  *
@@ -41,9 +41,6 @@ FSM_STATE_DECL(init);
 FSM_STATE_DECL(wait_for_start);
 /* movement states - buillon */
 FSM_STATE_DECL(aproach_buillon);
-//FSM_STATE_DECL(load_buillon);
-FSM_STATE_DECL(place_buillon);
-FSM_STATE_DECL(leave_buillon);
 /* Pushing the bottle */
 FSM_STATE_DECL(push_bottle);
 
@@ -66,7 +63,7 @@ FSM_STATE(init)
 void set_initial_position()
 {
        // TODO define initial position
-robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
+       robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
                                0);
 }
@@ -137,71 +134,14 @@ 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(load_buillon)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcSlow);
-                       robot_trajectory_add_final_point_trans(
-                               0.6,
-                               1.14,
-                               NO_TURN());
-                       break;
-               case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
-                       act_jaws(CATCH);
-                       break;
-               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:
@@ -220,9 +160,9 @@ 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;
@@ -250,6 +190,7 @@ int main()
        //robot.fsm.act.debug_states = 1;
 
        robot.fsm.main.state = &fsm_state_main_init;
+       //robot.team_color = BLUE;
        //robot.fsm.main.transition_callback = trans_callback;
        //robot.fsm.motion.transition_callback = move_trans_callback;