robofsm: Approach arena with lost bear

Add obstacle walls in the maze part.
Just go through the maze to arena and return back. Movement tuning is necessary.

diff --git a/src/robofsm/bear-rescue.cc b/src/robofsm/bear-rescue.cc
index 5fa6cc8ea4f6f6d2b719d488006ccc2a47444f86..07995e8b1c537559f261d08fd82c5fded2c68609 100644 (file)
--- a/src/robofsm/bear-rescue.cc
+++ b/src/robofsm/bear-rescue.cc
@@ -1,9 +1,9 @@
 /*
- * sick-day.cc       28/09/2012
+ * bear-rescue.cc
  *
- * Robot's control program intended for SICK robot day 2012 competition.
+ * Robot's control program intended for BEAR-RESCUE robot competition.
  *
- * Copyright: (c) 2012 Flamingos
+ * Copyright: (c) 2013 Flamingos
  *            CTU FEE - Department of Control Engineering
  * License: GNU GPL v.2
  */
@@ -84,33 +84,12 @@ FSM_STATE(wait_for_start)
                case EV_START:
                        /* start competition timer */
                        sem_post(&robot.start);
-                       FSM_TRANSITION(survey);
+                       FSM_TRANSITION(approach_arena);
                        break;
                case EV_TIMER:
                         FSM_TIMER(1000);
-                        act_omron_set_color(robot.team_color);
-
-                        switch (robot.team_color) {
-                        case TC_WHITE:
-                                robot.target_color[0] = 255;
-                                robot.target_color[1] = 255;
-                                robot.target_color[2] = 255;
-                                break;
-                        case TC_GREEN:
-                                robot.target_color[0] = 0;
-                                robot.target_color[1] = 252;
-                                robot.target_color[2] = 0;
-                                break;
-                        case TC_YELLOW:
-                                robot.target_color[0] = 255;
-                                robot.target_color[1] = 255;
-                                robot.target_color[2] = 19;
-                                break;
-                        }
-
                         if (robot.start_state == START_PLUGGED_IN)
                                actuators_home();
-
                        break;
                case EV_RETURN:
                case EV_MOTION_ERROR:

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index b417f8d67435f5b7ff464ddb255e277ebd971bf8..11759ed1527d2ab56f581bd72396021fac69d08e 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -51,7 +51,7 @@ const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIA
 
 void set_initial_position()
 {
-        robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
+        robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
 }
 
 void actuators_home()
@@ -313,20 +313,51 @@ FSM_STATE(move_around)
         }
 }
 
+FSM_STATE(approach_arena)
+{
+        switch (FSM_EVENT) {
+                case EV_ENTRY:
+                        DBG_PRINT_EVENT("approaching arena");
+
+                        robot_trajectory_new(&tcSlow);
+                        robot_trajectory_add_point_notrans(0.22, 0.9);
+                        robot_trajectory_add_point_notrans(0.65, 0.8);
+                        robot_trajectory_add_point_notrans(0.65, 0.5);
+                        robot_trajectory_add_point_notrans(1.15, 0.5);
+                        robot_trajectory_add_final_point_notrans(1.15, 1.35, ARRIVE_FROM(DEG2RAD(90), 0.1));
+                        break;
+                case EV_MOTION_ERROR:
+                        DBG_PRINT_EVENT("ERROR: position is not reachable!");
+                        FSM_TIMER(1000);
+                        break;
+                case EV_MOTION_DONE:
+                        FSM_TRANSITION(go_home);
+                        break;
+                case EV_TIMER:
+                case EV_EXIT:
+                        DBG_PRINT_EVENT("Unhandled event!");
+                        break;
+        }
+}
+
 FSM_STATE(go_home)
 {
         switch (FSM_EVENT) {
                 case EV_ENTRY:
                         DBG_PRINT_EVENT("homing");
-                        robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcFast);
+
+                        robot_trajectory_new(&tcSlow);
+                        robot_trajectory_add_point_notrans(1.15, 1.35);
+                        robot_trajectory_add_point_notrans(1.15, 0.5);
+                        robot_trajectory_add_point_notrans(0.65, 0.5);
+                        robot_trajectory_add_point_notrans(0.65, 0.8);
+                        robot_trajectory_add_point_notrans(0.22, 0.9);
+                        robot_trajectory_add_final_point_notrans(ROBOT_START_X_M, ROBOT_START_Y_M + 0.3, ARRIVE_FROM(DEG2RAD(270), 0.1));
                         break;
                 case EV_MOTION_ERROR:
                         DBG_PRINT_EVENT("ERROR: home position is not reachable!");
-                        FSM_TIMER(1000);
                         break;
                 case EV_TIMER:
-                        FSM_TRANSITION(go_home);
-                        break;
                 case EV_MOTION_DONE:
                 case EV_EXIT:
                         DBG_PRINT_EVENT("Mission completed!");

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index ae2b1c5378d5f6c8cf34bee372d1ca7d9ef7af46..4bedf4907d8a9e85f5ab0c345c51b7c02e5d87f9 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -6,10 +6,6 @@
 #include "roboevent.h"
 #include <fsm.h>
 
-#define HOME_POS_X_M            10
-#define HOME_POS_Y_M            1
-#define HOME_POS_ANG_DEG        90
-
 extern struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 
 /************************************************************************
@@ -18,6 +14,7 @@ extern struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 FSM_STATE_DECL(survey);
 FSM_STATE_DECL(approach_target);
 FSM_STATE_DECL(move_around);
+FSM_STATE_DECL(approach_arena);
 FSM_STATE_DECL(go_home);
 
 void set_initial_position(void);

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index d32db701fc956d617921021512f9621dbf269118..6134863064143fdffc7d874692eed40c8931679b 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -59,9 +59,9 @@ static void int_handler(int sig)
 
 void fill_in_known_areas_in_map()
 {
-       /* Restrict playground space to circle */
-        ShmapSetRectangleFlag(0.0, 0.0, PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M, MAP_FLAG_WALL, 0);
-        ShmapSetCircleFlag(PLAYGROUND_WIDTH_M/2, PLAYGROUND_HEIGHT_M/2, PLAYGROUND_WIDTH_M/2, 0, MAP_FLAG_WALL);
+        ShmapSetRectangleFlag(0.0, PLAYGROUND_HEIGHT_M / 2.0, PLAYGROUND_WIDTH_M - 0.5, PLAYGROUND_HEIGHT_M / 2.0, MAP_FLAG_WALL, 0);
+        ShmapSetRectangleFlag(0.4, 0.0, 0.4, PLAYGROUND_HEIGHT_M / 4.0, MAP_FLAG_WALL, 0);
+        ShmapSetRectangleFlag(0.9, PLAYGROUND_HEIGHT_M / 4.0, 0.9, PLAYGROUND_HEIGHT_M / 2.0, MAP_FLAG_WALL, 0);
 }
 
 static void trans_callback(struct robo_fsm *fsm)