summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-13 00:22:28 +0200
committerNicolas Schodet2010-04-13 00:22:28 +0200
commit621fd67190770e3f8bace929d5fb9f5633220475 (patch)
tree050e376bfd1e4b8320549cc24d38ca27e95f767e
parentb87e61cba74767dcb84d027c44efd27032141462 (diff)
digital/io/src: add blocking obstacle event, closes #129
-rw-r--r--digital/io/src/ai_move_cb.c1
-rw-r--r--digital/io/src/main.c4
-rw-r--r--digital/io/src/main.h4
-rw-r--r--digital/io/src/move.c16
-rw-r--r--digital/io/src/move.h6
-rw-r--r--digital/io/src/radar.c37
-rw-r--r--digital/io/src/radar.h22
7 files changed, 90 insertions, 0 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index ffd6f459..b52d8279 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -119,6 +119,7 @@ move_get_next_position (void)
asserv_goto_xya (dst.x, dst.y, move_data.final.a,
move_data.backward_movement_allowed);
}
+ move_data.step = dst;
TRACE (TRACE_MOVE__GO_TO, (u16) current_pos.v.x,
(u16) current_pos.v.y, current_pos.a, dst.x, dst.y,
move_data.final.a);
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index bc14da10..e06b5139 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -52,6 +52,7 @@
#include "playground.h"
#include "contact.h"
#include "init.h"
+#include "move.h"
#include "top.h"
#include "io.h"
@@ -164,6 +165,9 @@ main_event_to_fsm (void)
/* Post the event */
FSM_HANDLE_EVENT (&ai_fsm, save_event);
}
+
+ /* Check obstacles. */
+ move_check_obstacles ();
/* TODO: Check other sensors */
}
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 15f1fce0..0c2083ba 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -33,4 +33,8 @@
*/
extern uint8_t main_post_event_for_top_fsm;
+extern vect_t main_obstacles_pos[2];
+
+extern uint8_t main_obstacles_nb;
+
#endif /* main_h */
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 69f71b31..f0996070 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -25,6 +25,9 @@
#include "common.h"
#include "move.h"
#include "fsm.h"
+#include "radar.h"
+#include "asserv.h"
+#include "main.h"
/**
* Internal data used by the move FSM.
@@ -42,3 +45,16 @@ move_start (position_t position, uint8_t backward)
fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
+void
+move_check_obstacles (void)
+{
+ if (fsm_can_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front))
+ {
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos,
+ main_obstacles_nb))
+ fsm_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front);
+ }
+}
+
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 8ea9eed4..a208997f 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -34,6 +34,8 @@ struct move_data_t
{
/** Final position. */
position_t final;
+ /** Next step. */
+ vect_t step;
/** Backward direction allowed flag. */
uint8_t backward_movement_allowed;
/** Try again counter. */
@@ -56,4 +58,8 @@ extern struct move_data_t move_data;
void
move_start (position_t position, uint8_t backward);
+/** Check for blocking obstacles. */
+void
+move_check_obstacles (void);
+
#endif /* move_h */
diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c
index 93aaa047..0d3611dc 100644
--- a/digital/io/src/radar.c
+++ b/digital/io/src/radar.c
@@ -141,3 +141,40 @@ radar_update (const position_t *robot_pos, vect_t *obs_pos)
return obs_nb;
}
+uint8_t
+radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
+ const vect_t *obs_pos, uint8_t obs_nb)
+{
+ uint8_t i;
+ /* Stop here if no obstacle. */
+ if (!obs_nb)
+ return 0;
+ vect_t vd = *dest_pos; vect_sub (&vd, robot_pos);
+ uint16_t d = vect_norm (&vd);
+ /* If destination is realy near, stop here. */
+ if (d < RADAR_EPSILON_MM)
+ return 0;
+ /* To save divisions, multiply limits by vd (and vdn) length. */
+ vect_t vdn = vd; vect_normal (&vdn);
+ int32_t limit = (uint32_t) d * (BOT_SIZE_FRONT + RADAR_STOP_MM
+ + RADAR_OBSTACLE_RADIUS_MM);
+ int32_t limitn = (uint32_t) d * (BOT_SIZE_SIDE + RADAR_CLEARANCE_MM
+ + RADAR_OBSTACLE_RADIUS_MM);
+ /* Now, look at obstacles. */
+ for (i = 0; i < obs_nb; i++)
+ {
+ /* Vector from robot to obstacle. */
+ vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos);
+ /* Test if within rectangle (see RADAR_CLEARANCE_MM comment). */
+ int32_t dp = vect_dot_product (&vd, &vo);
+ if (dp < 0 || dp > limit)
+ continue;
+ int32_t dpn = vect_dot_product (&vdn, &vo);
+ if (dpn < -limitn || dpn > limitn)
+ continue;
+ /* Else, obstacle is blocking. */
+ return 1;
+ }
+ return 0;
+}
+
diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h
index d5fa8b78..e486b069 100644
--- a/digital/io/src/radar.h
+++ b/digital/io/src/radar.h
@@ -39,9 +39,31 @@
* added to position obstacle center. */
#define RADAR_OBSTACLE_RADIUS_MM 150
+/** Stop distance. Distance under which an obstacle is considered harmful when
+ * moving. */
+#define RADAR_STOP_MM 350
+
+/** Clearance distance. Distance over which an obstacle should be to the side
+ * when moving.
+ *
+ * OK, more explanations: when moving, a rectangle is placed in front of the
+ * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM +
+ * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered
+ * in the way. */
+#define RADAR_CLEARANCE_MM 100
+
+/** Destination distance near enough so that obstacles could be ignored. */
+#define RADAR_EPSILON_MM 70
+
/** Update radar view. Return the number of obstacles found. Obstacles
* positions are returned in obs_pos. */
uint8_t
radar_update (const position_t *robot_pos, vect_t *obs_pos);
+/** Return non zero if there is a blocking obstacle near the robot while going
+ * to a destination point. */
+uint8_t
+radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos,
+ const vect_t *obs_pos, uint8_t obs_nb);
+
#endif /* radar_h */