robofsm: competition2012

Finished first idea of competition trajectory.

Homologation + collecting the buillons from the nearest totem. After the buillons are collected, they are transported into captain's cabin.

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index c3f31434a9467592f0b4f5f8cdbd2e0b655bca6a..6a513c0110a1d4735f1e9ed02d25b57797a8bcac 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -7,9 +7,9 @@ default_CONFIG = CONFIG_LOCK_CHECKING=n HAVE_PRIO_INHERIT=y
 config_include_HEADERS = robot_config.h
 robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 
-#bin_PROGRAMS += competition
-competition_SOURCES = competition.cc   \
-                     common-states.cc
+bin_PROGRAMS += competition
+competition_SOURCES = competition2012.cc#      \
+#                    common-states.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation2012.cc

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index a48f94ba85ba4eb9ad79095bdb4933e41677d3fd..2c8ab1c0fd57ba49cc59ea7f36d8860643a476c0 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -49,6 +49,11 @@ 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);
 
 FSM_STATE(init)
@@ -280,14 +285,13 @@ FSM_STATE(leave_totem)
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(go_home);
+                       FSM_TRANSITION(goto_totem2);
                default:
                        break;
        }
 }
 
-
-FSM_STATE(go_home)
+FSM_STATE(goto_totem2)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
@@ -297,10 +301,62 @@ FSM_STATE(go_home)
                                0.6);
                        robot_trajectory_add_point_trans(
                                0.64,
-                               1.0);
+                               1.3);
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               1.6,
+                               ARRIVE_FROM(DEG2RAD(270),0.1));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(approach_totem2);
+               default:
+                       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)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               1.6,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(go_home);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(go_home)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
                        robot_trajectory_add_final_point_trans(
-                               0.35,
-                               1.0,
+                               0.5 + ROBOT_AXIS_TO_FRONT_M,
+                               1.75,
                                ARRIVE_FROM(DEG2RAD(180),0.1));
                        break;
                case EV_MOTION_DONE: