From 621fd67190770e3f8bace929d5fb9f5633220475 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 13 Apr 2010 00:22:28 +0200 Subject: digital/io/src: add blocking obstacle event, closes #129 --- digital/io/src/ai_move_cb.c | 1 + digital/io/src/main.c | 4 ++++ digital/io/src/main.h | 4 ++++ digital/io/src/move.c | 16 ++++++++++++++++ digital/io/src/move.h | 6 ++++++ digital/io/src/radar.c | 37 +++++++++++++++++++++++++++++++++++++ digital/io/src/radar.h | 22 ++++++++++++++++++++++ 7 files changed, 90 insertions(+) (limited to 'digital') 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 */ -- cgit v1.2.3