summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/move.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/guybrush/move.c')
-rw-r--r--digital/io-hub/src/guybrush/move.c80
1 files changed, 68 insertions, 12 deletions
diff --git a/digital/io-hub/src/guybrush/move.c b/digital/io-hub/src/guybrush/move.c
index 3c4c7f46..056ffeb2 100644
--- a/digital/io-hub/src/guybrush/move.c
+++ b/digital/io-hub/src/guybrush/move.c
@@ -36,9 +36,15 @@
#include "path.h"
#include "modules/utils/utils.h"
+#include "modules/proto/proto.h"
#include <math.h>
+#include "playground_2012.h"
+
+/** Number of tries to reach destination. */
+#define MOVE_TRY_AGAIN 1
+
/** Move context. */
struct move_t
{
@@ -80,7 +86,7 @@ move_start (position_t position, uint8_t backward)
move_data.final_move = 0;
move_data.shorten = 0;
/* Reset try counter. */
- move_data.try_again_counter = 3;
+ move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
fsm_queue_post_event (FSM_EVENT (AI, move_start));
}
@@ -95,7 +101,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
move_data.final_move = 0;
move_data.shorten = shorten;
/* Reset try counter. */
- move_data.try_again_counter = 3;
+ move_data.try_again_counter = MOVE_TRY_AGAIN;
/* Start the FSM. */
fsm_queue_post_event (FSM_EVENT (AI, move_start));
}
@@ -112,8 +118,12 @@ move_obstacles_update (void)
{
uint8_t i;
for (i = 0; i < main_obstacles_nb; i++)
+ {
path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
+ proto_send2w ('o', main_obstacles_pos[i].x,
+ main_obstacles_pos[i].y);
+ }
}
uint8_t
@@ -138,6 +148,8 @@ FSM_STATES (
MOVE_ROTATING,
/* Moving to a position (intermediate or final). */
MOVE_MOVING,
+ /* Brake when a obstacle is seen. */
+ MOVE_BRAKE,
/* Moving backward to go away from what is blocking the bot. */
MOVE_MOVING_BACKWARD_TO_TURN_FREELY,
/* Waiting for obstacle to disappear. */
@@ -375,24 +387,50 @@ FSM_TRANS (MOVE_MOVING, robot_move_success,
//return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
}
+/** Test if a point is ok to move back. */
+static uint8_t
+move_test_point (vect_t p, int16_t margin)
+{
+ return p.x >= margin && p.x < PG_WIDTH - margin
+ && p.y >= margin && p.y < PG_LENGTH - margin
+ && !(p.y >= PG_TOTEM_Y - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.y < PG_TOTEM_Y + PG_TOTEM_WIDTH_MM / 2 + margin
+ && p.x >= PG_TOTEM_LEFT_X - PG_TOTEM_WIDTH_MM / 2 - margin
+ && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + margin);
+}
+
static void
move_moving_backward_to_turn_freely (void)
{
move_data.final_move = 0;
+ int16_t dist, back_dist, margin;
+ if (asserv_get_last_moving_direction () == DIRECTION_FORWARD)
+ {
+ dist = BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS;
+ back_dist = -300;
+ margin = BOT_SIZE_BACK_RADIUS;
+ }
+ else
+ {
+ dist = -(BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS);
+ back_dist = 300;
+ margin = BOT_SIZE_RADIUS;
+ }
/* Assume there is an obstacle in front of the robot. */
position_t robot_pos;
asserv_get_position (&robot_pos);
vect_t obstacle_pos;
- int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD
- ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS
- : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS);
vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
vect_translate (&obstacle_pos, &robot_pos.v);
path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0,
MOVE_OBSTACLE_VALIDITY);
/* Move backward to turn freely. */
- asserv_move_linearly (asserv_get_last_moving_direction ()
- == DIRECTION_FORWARD ? -300 : 300);
+ vect_t back_pos;
+ vect_from_polar_uf016 (&back_pos, back_dist, robot_pos.a);
+ vect_translate (&back_pos, &robot_pos.v);
+ if (!move_test_point (back_pos, margin))
+ back_dist /= 8;
+ asserv_move_linearly (back_dist);
}
FSM_TRANS (MOVE_MOVING,
@@ -410,19 +448,37 @@ FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500,
return FSM_NEXT_TIMEOUT (MOVE_MOVING);
}
-FSM_TRANS (MOVE_MOVING, obstacle_in_front,
- tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
- tryout, MOVE_IDLE)
+FSM_TRANS (MOVE_MOVING, obstacle_in_front, MOVE_BRAKE)
{
move_data.final_move = 0;
asserv_stop_motor ();
+ return FSM_NEXT (MOVE_MOVING, obstacle_in_front);
+}
+
+FSM_TRANS (MOVE_BRAKE, robot_move_success,
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ tryout, MOVE_IDLE)
+{
+ if (--move_data.try_again_counter == 0)
+ {
+ fsm_queue_post_event (FSM_EVENT (AI, move_failure));
+ return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryout);
+ }
+ else
+ return FSM_NEXT (MOVE_BRAKE, robot_move_success, tryagain);
+}
+
+FSM_TRANS (MOVE_BRAKE, robot_move_failure,
+ tryagain, MOVE_WAIT_FOR_CLEAR_PATH,
+ tryout, MOVE_IDLE)
+{
if (--move_data.try_again_counter == 0)
{
fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryout);
}
else
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+ return FSM_NEXT (MOVE_BRAKE, robot_move_failure, tryagain);
}
FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)