robofsm: basic strategy rewrite (using the skeleton created by Michal in commit e3afc...

- common-states.{cc,h}: prepare states for three subautomatons which do
	- oranges collecting
	- unloading the oranges
	- rushing particular corns one by one
- the strategy_opp_oranges is now "almost" usable

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 973ce8306042aa65d8f813dc7b86def69a055087..6e977a5553bb77dde493cff38a1dd82df634dcd8 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -16,9 +16,15 @@
 #include <trgen.h>
 #include "match-timing.h"
 #include "eb2010misc.h"
+#include <stdbool.h>
 
 #include "common-states.h"
 
+/************************************************************************
+ * Functions used in and called from all the (almost identical)
+ * "wait for start" states in particular strategies.
+ ************************************************************************/
+
 static void set_initial_position()
 {
        robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
@@ -31,8 +37,6 @@ static void actuators_home()
        act_vidle(VIDLE_UP);
 }
 
-
-
 void start_entry()
 {
        pthread_t thid;
@@ -64,76 +68,54 @@ void start_exit()
 }
 
 /************************************************************************
- * Trajectory constraints used, are initialized in the init state
+ * Trajectory constraints used; They are  initialized in the main() function in competition.cc
  ************************************************************************/
 
 struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
+#define VIDLE_TIMEOUT 2000
 
+/************************************************************************
+ * States that form the "collect some oranges" subautomaton. Calling automaton
+ * SHOULD ALWAYS call the "approach_the_slope" state.
+ ************************************************************************/
 
-FSM_STATE(zvedej_vidle)
-{
-       static int cnt = 0;
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-               case EV_TIMER:
-                       FSM_TIMER(500);
-                       act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN);
-                       printf("--------------------cnt: %d\n", cnt);
-                       cnt++;
-                       if(cnt >= 3) {
-                               robot_exit();
-                               //FSM_TRANSITION(sledge_down);
-                       }
-                       break;
-               case EV_START:
-               case EV_RETURN:
-               case EV_ACTION_DONE:
-               case EV_ACTION_ERROR:
-               case EV_MOTION_DONE:
-               case EV_GOAL_NOT_REACHABLE:
-                       DBG_PRINT_EVENT("unhandled event");
-               case EV_EXIT:
-                       break;
+bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) {
+       bool ret;
+       if (which_slope == MINE) {
+               ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
+       } else if (which_slope == OPPONENTS) {
+               ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5;
+       } else {
+               printf("ERROR: unknown side;"); robot_exit();
        }
+       return ret;
 }
 
-static enum strategy {
-       OPPONENTS_ORAGES_TOO,
-       CORNS_ONLY
-} strategy = OPPONENTS_ORAGES_TOO;
-
 static enum which_side which_slope;
-static int slope_cnt = 0;
+
+/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */
 FSM_STATE(approach_the_slope)
 {
        switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       if (strategy == OPPONENTS_ORAGES_TOO) {
-                               if (slope_cnt == 0) {
-                                       which_slope = MINE;
+               case EV_ENTRY: {
+                               which_slope = (enum which_side) FSM_EVENT_INT;
+                               double x, y, phi;
+                               robot_get_est_pos_trans(&x, &y, &phi);
+                               /* decide */
+                               bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(which_slope, x, y);
+                               /* if necessary, approach the slope */
+                               if (ready_to_climb_the_slope) {
                                        FSM_TRANSITION(climb_the_slope);
-                               } else if (slope_cnt == 1) {
-                                       which_slope = OPPONENTS;
+                               } else {
                                        robot_goto_trans(
-                                               x_coord(0.3, which_slope),
-                                               PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03,
-                                               ARRIVE_FROM(DEG2RAD(0), 0.02),
+                                               x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.05, which_slope),
+                                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05,
+                                               ARRIVE_FROM(DEG2RAD(0),0.05),
                                                &tcFast);
-                               } else {
-                                       which_slope = MINE;
-                                       FSM_TRANSITION(approach_next_corn);
-                               }
-                       } else if (strategy == CORNS_ONLY) {
-                               if (slope_cnt == 0) {
-                                       which_slope = MINE;
-                                       FSM_TRANSITION(climb_the_slope);
-                               } else if (slope_cnt == 1) {
-                                       FSM_TRANSITION(approach_next_corn);
                                }
+                               break;
                        }
-                       slope_cnt++;
-                       break;
                case EV_MOTION_DONE:
                        FSM_TRANSITION(climb_the_slope);
                        break;
@@ -149,38 +131,39 @@ FSM_STATE(approach_the_slope)
        }
 }
 
+void inline enable_switches(bool enabled)
+{
+       robot.use_left_switch = enabled;
+       robot.use_right_switch = enabled;
+       robot.use_back_switch = enabled;
+}
+
 FSM_STATE(climb_the_slope)
 {
        switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       // disables using side switches on bumpers when going up
-                       robot.use_left_switch = false;
-                       robot.use_right_switch = false;
-                       robot.use_back_switch = false;
-                       act_vidle(VIDLE_DOWN);
-                       robot.ignore_hokuyo = true;
-                       robot_trajectory_new_backward(&tcSlow);
-                       if (which_slope == MINE) {
-                               robot_trajectory_add_point_trans(
-                                       x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, which_slope),
-                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
-                       } else {
+               case EV_ENTRY: {
+                               // disables using side switches on bumpers when going up
+                               enable_switches(false);
+                               act_vidle(VIDLE_DOWN);
+                               robot.ignore_hokuyo = true;
+                               /* create the trajectory and go */
+                               robot_trajectory_new_backward(&tcSlow);
                                robot_trajectory_add_point_trans(
                                        x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope),
                                        PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01);
+                               robot_trajectory_add_final_point_trans(
+                                       x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
+                                       PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
+                                       NO_TURN());
+                               break;
                        }
-                       /* position for collecting oranges*/
-                       robot_trajectory_add_final_point_trans(
-                               x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope),
-                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
-                               NO_TURN());
-                       break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(2000);
+                       FSM_TIMER(VIDLE_TIMEOUT);
                        act_vidle(VIDLE_UP);
                        break;
                case EV_START:
                case EV_TIMER:
+                       // FIXME: respond to VIDLE EVENT
                        FSM_TRANSITION(sledge_down);
                        break;
                case EV_RETURN:
@@ -201,6 +184,7 @@ FSM_STATE(sledge_down)
                        robot_trajectory_add_point_trans(
                                x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope),
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
+                       /*
                        robot_trajectory_add_point_trans(
                                x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
                                PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
@@ -208,11 +192,19 @@ FSM_STATE(sledge_down)
                                x_coord(0.50, which_slope),
                                PLAYGROUND_HEIGHT_M - 0.6,
                                NO_TURN());
+                       */
+                       robot_trajectory_add_point_trans(
+                               x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.20, which_slope),
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08);
+                       robot_trajectory_add_final_point_trans(
+                               x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope),
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.14,
+                               NO_TURN());
                        break;
                case EV_MOTION_DONE:
+                       /* just for sure, try to close it one more time */
                        act_vidle(VIDLE_UP);
-                       FSM_TRANSITION(to_container_diag);
-                       //FSM_TRANSITION(to_container_ortho);
+                       SUBFSM_RET(NULL);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -223,9 +215,7 @@ FSM_STATE(sledge_down)
                        DBG_PRINT_EVENT("unhandled event");
                case EV_EXIT:
                        // enables using side switches on bumpers
-                       robot.use_left_switch = true;
-                       robot.use_right_switch = true;
-                       robot.use_back_switch = true;
+                       enable_switches(true);
                        robot.ignore_hokuyo = false;
                        robot.check_turn_safety = true;
 
@@ -233,10 +223,15 @@ FSM_STATE(sledge_down)
        }
 }
 
-FSM_STATE(to_container_diag)
+/************************************************************************
+ * The "unload our oranges" subautomaton
+ ************************************************************************/
+
+FSM_STATE(to_cntainer_and_unld)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
+                       /*
                        if (which_slope == MINE) {
                                robot_trajectory_new(&tcFast);
                                // face the rim with front of the robot
@@ -244,18 +239,17 @@ FSM_STATE(to_container_diag)
                                // face the rim with back of the robot
                                robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
                                robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
-                       } else {
+                       } else { */
                                robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast);
-                       }
+                       //}
                        break;
                case EV_MOTION_DONE:
-                       FSM_TIMER(6000);
+                       FSM_TIMER(3000); // FIXME: test this
                        act_vidle(VIDLE_VYSIP);
                        break;
                case EV_TIMER:
                        act_vidle(VIDLE_UP);
-                       FSM_TRANSITION(approach_the_slope);
-                       //FSM_TRANSITION(approach_next_corn);
+                       SUBFSM_RET(NULL);
                        break;
                case EV_START:
                case EV_RETURN:
@@ -268,38 +262,9 @@ FSM_STATE(to_container_diag)
        }
 }
 
-FSM_STATE(to_container_ortho)
-{
-       switch(FSM_EVENT) {
-               case EV_ENTRY:
-                       robot_trajectory_new(&tcFast);
-                       robot_trajectory_add_point_trans(
-                               SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
-                               PLAYGROUND_HEIGHT_M - 0.355);
-                       robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65);
-                       robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75);
-                       robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7);
-                       robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9);
-
-                       // face the rim with front of the robot
-                       //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
-                       // face the rim with back of the robot
-                       robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90)));
-                       break;
-               case EV_MOTION_DONE:
-                       act_vidle(VIDLE_VYSIP);
-                       break;
-               case EV_START:
-               case EV_TIMER:
-               case EV_RETURN:
-               case EV_ACTION_DONE:
-               case EV_ACTION_ERROR:
-               case EV_GOAL_NOT_REACHABLE:
-                       DBG_PRINT_EVENT("unhandled event");
-               case EV_EXIT:
-                       break;
-       }
-}
+/************************************************************************
+ * The "collect corns" subautomaton
+ ************************************************************************/
 
 static enum where_to_go {
        CORN,
@@ -310,9 +275,8 @@ static enum where_to_go {
 
 static struct corn *corn_to_get;
 
-FSM_STATE(experiment_decider)
+FSM_STATE(rush_corns_decider)
 {
-       
        switch(FSM_EVENT) {
                case EV_ENTRY:
                        if (where_to_go == CORN) {
@@ -363,7 +327,7 @@ FSM_STATE(approach_next_corn)
                        }
                case EV_MOTION_DONE:
                        cnt++;
-                       FSM_TRANSITION(experiment_decider);
+                       FSM_TRANSITION(rush_corns_decider);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -392,7 +356,7 @@ FSM_STATE(rush_the_corn)
                        where_to_go = TURN_AROUND;
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(experiment_decider);
+                       FSM_TRANSITION(rush_corns_decider);
                        break;
                case EV_START:
                case EV_TIMER:
@@ -416,7 +380,7 @@ FSM_STATE(turn_around)
                        break;
                case EV_MOTION_DONE:
                        where_to_go = CORN;
-                       FSM_TRANSITION(experiment_decider);
+                       FSM_TRANSITION(rush_corns_decider);
                        break;
                case EV_START:
                case EV_TIMER:

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index b4fed1ba13210d46f627432c69821400a0edeaa9..8e47fb7747c7e18f91d910ff055b6b1e7e89e60b 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -14,13 +14,11 @@ FSM_STATE_DECL(start_opp_corn);
 
 FSM_STATE_DECL(climb_the_slope);
 FSM_STATE_DECL(sledge_down);
-FSM_STATE_DECL(to_container_diag);
-FSM_STATE_DECL(to_container_ortho);
-FSM_STATE_DECL(experiment_decider);
+FSM_STATE_DECL(to_cntainer_and_unld);
+FSM_STATE_DECL(rush_corns_decider);
 FSM_STATE_DECL(approach_next_corn);
 FSM_STATE_DECL(rush_the_corn);
 FSM_STATE_DECL(turn_around);
-FSM_STATE_DECL(zvedej_vidle);
 FSM_STATE_DECL(approach_the_slope);
 
 void start_entry();

diff --git a/src/robofsm/strategy_opp_corn.cc b/src/robofsm/strategy_opp_corn.cc
index fabf6cc11fd4e89925988e2aeee974ded12d22e1..d6ce4666f9c5762d5ed89c90448fc677eaad10a4 100644 (file)
--- a/src/robofsm/strategy_opp_corn.cc
+++ b/src/robofsm/strategy_opp_corn.cc
@@ -40,7 +40,7 @@ FSM_STATE_DECL(opp_corns);
 FSM_STATE(pick_our_oranges)
 {
        switch (FSM_EVENT) {
-       case EV_ENTRY:  SUBFSM_TRANSITION(climb_the_slope, NULL); break;
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_the_slope, (void*) MINE); break;
        case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
        default: break;
        }
@@ -49,7 +49,7 @@ FSM_STATE(pick_our_oranges)
 FSM_STATE(unload_oranges)
 {
        switch (FSM_EVENT) {
-       case EV_ENTRY:  SUBFSM_TRANSITION(to_container_diag, NULL); break;
+       case EV_ENTRY:  SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
        case EV_RETURN: FSM_TRANSITION(opp_corns); break;
        default: break;
        }

diff --git a/src/robofsm/strategy_opp_oranges.cc b/src/robofsm/strategy_opp_oranges.cc
index 87a496a1aa44cc4e74516353cb8695d57ffff77e..0a21868a3ef2862652301ab529f3f1109149b085 100644 (file)
--- a/src/robofsm/strategy_opp_oranges.cc
+++ b/src/robofsm/strategy_opp_oranges.cc
@@ -34,13 +34,13 @@ FSM_STATE(start_opp_oranges)
 #define FSM_STATE_VISIBILITY static
 
 FSM_STATE_DECL(unload_oranges);
-FSM_STATE_DECL(opp_orange);
+FSM_STATE_DECL(pick_opp_oranges);
 FSM_STATE_DECL(opp_corns);
 
 FSM_STATE(pick_our_oranges)
 {
        switch (FSM_EVENT) {
-       case EV_ENTRY:  SUBFSM_TRANSITION(climb_the_slope, (void*)0); break;
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_the_slope, (void*) MINE); break;
        case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
        default: break;
        }
@@ -49,17 +49,18 @@ FSM_STATE(pick_our_oranges)
 FSM_STATE(unload_oranges)
 {
        switch (FSM_EVENT) {
-       case EV_ENTRY:  SUBFSM_TRANSITION(to_container_diag, NULL); break;
-       case EV_RETURN: FSM_TRANSITION(opp_orange); break;
+       case EV_ENTRY:  SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break;
+       /* FIXME: do something more meaningfull the next time */
+       case EV_RETURN: FSM_TRANSITION(pick_opp_oranges); break;
        default: break;
        }
 }
 
-FSM_STATE(opp_orange)
+FSM_STATE(pick_opp_oranges)
 {
        switch (FSM_EVENT) {
-       case EV_ENTRY:  SUBFSM_TRANSITION(climb_the_slope, (void*)1); break;
-       case EV_RETURN: FSM_TRANSITION(opp_corns); break;
+       case EV_ENTRY:  SUBFSM_TRANSITION(approach_the_slope, (void*) OPPONENTS); break;
+       case EV_RETURN: FSM_TRANSITION(unload_oranges); break;
        default: break;
        }
 }