summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/main.c14
-rw-r--r--digital/io/src/main.h8
-rw-r--r--digital/io/src/move.fsm66
-rw-r--r--digital/io/src/move.h2
-rw-r--r--digital/io/src/move_cb.c219
5 files changed, 243 insertions, 66 deletions
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index e5d25cf3..b40eefd5 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -76,6 +76,12 @@ uint8_t main_post_event_for_top_fsm = 0xFF;
uint16_t main_sharp_ignore_event;
/**
+ * Post an event for the main loop to wake up the move FSM in a certain count
+ * of cycles.
+ */
+uint16_t main_move_wait_cycle;
+
+/**
* Sharps stats counters.
*/
uint8_t main_stats_sharps, main_stats_sharps_cpt;
@@ -182,6 +188,9 @@ main_loop (void)
/* Update the ignore sharp event flag */
if (main_sharp_ignore_event)
main_sharp_ignore_event--;
+ /* Update wait flag for move FSM */
+ if (main_move_wait_cycle)
+ main_move_wait_cycle--;
/* Update sharp module if required and only every
* MAIN_SHARP_UPDATE_FREQ cycles */
if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ)
@@ -282,6 +291,11 @@ main_loop (void)
}
}
}
+ /* Wait flag for move FSM */
+ if (!main_move_wait_cycle)
+ {
+ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished);
+ }
/* TODO: Check other sensors */
}
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 24359c6d..96efb10a 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -40,4 +40,12 @@ extern uint8_t main_post_event_for_top_fsm;
*/
extern uint16_t main_sharp_ignore_event;
+/**
+ * Post an event for the main loop to wake up the move FSM in a certain count
+ * of cycles.
+ * This flag is used by the move FSM to wait a certain amount of time before
+ * checking on the sharps again.
+ */
+extern uint16_t main_move_wait_cycle;
+
#endif /* main_h */
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index ab55d1d4..317f25d8 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -13,6 +13,8 @@ States:
moving backward to go away from what is blocking the bot
MOVING_TO_INTERMEDIATE_POSITION
moving to an intermediate position to try avoiding the obstacle
+ WAIT_FOR_CLEAR_PATH
+ waiting for obstacle to disapear
Events:
start
@@ -23,6 +25,8 @@ Events:
the bot has finished to moved and arrived at the desired position
bot_move_obstacle
the bot has seen something (with the sharps)
+ wait_finished
+ we have wait the desired time
IDLE:
start -> MOVING_TO_FINAL_POSITION
@@ -32,36 +36,64 @@ MOVING_TO_FINAL_POSITION:
bot_move_succeed -> IDLE
post an event for the top FSM to tell we have finished
bot_move_failed -> MOVING_BACKWARD
- store the current position of the obstacle
+ compute the obstacle position
+ store current moving direction (for possible failed of path module)
move backward to turn freely
- bot_move_obstacle -> MOVING_TO_INTERMEDIATE_POSITION
+ bot_move_obstacle: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION
+ compute the obstacle position
+ get next intermediate position from path module
+ go to next intermediate position
+ bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
compute the obstacle position
- give the obstacle position to the path module
- give the current position of the bot to the path module
- get the intermediate position from path module
+ get next intermediate position from path module failed
+ store current moving direction
+ stop the bot
+ post an event for the top FSM to be waked up later
MOVING_BACKWARD:
# TODO
# We ignore obstacle when moving backward
- bot_move_succeed -> MOVING_TO_INTERMEDIATE_POSITION
- give the obstacle position to the path module
- give the current position of the bot to the path module
- get the intermediate position from path module
- bot_move_failed -> MOVING_TO_INTERMEDIATE_POSITION
- do the same as when we succeed
- give the obstacle position to the path module
- give the current position of the bot to the path module
- get the intermediate position from path module
+ bot_move_succeed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION
+ get next intermediate position from path module
+ bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
+ get next intermediate position from path module, failed
+ stop the bot
+ post an event for the top FSM to be waked up later
+# Do the same as when we succeed
+ bot_move_failed: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION
+ get next intermediate position from path module
+ bot_move_failed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
+ get next intermediate position from path module, failed
+ stop the bot
+ post an event for the top FSM to be waked up later
MOVING_TO_INTERMEDIATE_POSITION:
- bot_move_obstacle -> .
+ bot_move_obstacle: intermediate_path_found -> .
compute the obstacle position
- give the obstacle position to the path module
- go to the next intermediate position computed by the path module
+ get next intermediate position from path module
+ go to next intermediate position
+ bot_move_obstacle: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
+ compute the obstacle position
+ get next intermediate position from path module failed
+ store current moving direction
+ stop the bot
+ post an event for the top FSM to be waked up later
bot_move_succeed: final_position -> IDLE
post an event for the top FSM to tell we have finished
bot_move_succeed: position_intermediary -> .
go to the next intermediate position computed by the path module
+ bot_move_succeed: no_intermediate_path_found -> WAIT_FOR_CLEAR_PATH
+ store current moving direction
+ stop the bot
+ post an event for the top FSM to be waked up later
bot_move_failed -> MOVING_BACKWARD
store the current position of the obstacle
move backward to turn freely
+
+WAIT_FOR_CLEAR_PATH:
+ wait_finished: obstacle -> .
+ check for obstacle using stored moving direction
+ post an event for the top FSM to be waked up later
+ wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION
+ check for obstacle using stored moving direction
+ try to go the final position
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 5a328950..1b1621af 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -49,6 +49,8 @@ struct move_data_t
move_position_t obstacle;
/** Backward direction allowed flag. */
uint8_t backward_movement_allowed;
+ /** Cached moving direction of the bot when blocked. */
+ uint8_t cached_moving_direction;
};
/**
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index 5b1c63a6..8393c60d 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -30,6 +30,7 @@
#include "asserv.h"
#include "playground.h"
#include "move.h"
+#include "sharp.h"
#include "main.h" /* main_post_event_for_top_fsm */
#include "modules/math/fixed/fixed.h" /* fixed_* */
@@ -75,10 +76,19 @@
#define MOVE_MAIN_IGNORE_SHARP_EVENT (3 * 225)
/**
+ * Number of cycles to wait before trying to read the sharps values again when
+ * we are stopped.
+ */
+#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (50)
+
+/**
* Easier function to get the next intermediate positon from the path module.
* @param dst new destination position computed by the path module
+ * @return
+ * - 0 if no path could be found ;
+ * - 1 if a path has been found.
*/
-void
+uint8_t
move_get_next_position (move_position_t *dst)
{
/* Get the current position */
@@ -94,12 +104,11 @@ move_get_next_position (move_position_t *dst)
if (!path_get_next (&dst->x, &dst->y))
{
DPRINTF ("Could not compute any path to avoid obstacle!\n");
- /* If it failed, try original destination */
- dst->x = move_data.final.x;
- dst->y = move_data.final.y;
+ return 0;
}
main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT;
DPRINTF ("Computed path is (%d ; %d)\n", dst->x, dst->y);
+ return 1;
}
/**
@@ -144,17 +153,52 @@ move_obstacle_here (void)
/**
* Unique function after moving backward to have unique code.
+ * @return the value of the move_get_next_position.
*/
-void
+uint8_t
move_after_moving_backward (void)
{
/* Give the current position of the bot to the path module */
- move_get_next_position (&move_data.intermediate);
- /* Go to the next position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ if (move_get_next_position (&move_data.intermediate))
+ {
+ /* Go to the next position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ else
+ asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ return 1;
+ }
+ else
+ return 0;
+}
+
+/*
+ * WAIT_FOR_CLEAR_PATH =wait_finished=>
+ * no_obstacle => MOVING_TO_FINAL_POSITION
+ * check for obstacle using stored moving direction
+ * try to go the final position
+ * obstacle => WAIT_FOR_CLEAR_PATH
+ * check for obstacle using stored moving direction
+ * post an event for the top FSM to be waked up later
+ */
+fsm_branch_t
+move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
+{
+ if (!sharp_path_obstrued (move_data.cached_moving_direction))
+ {
+ /* Try to go to the final position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.final.x, move_data.final.y);
+ else
+ asserv_goto (move_data.final.x, move_data.final.y);
+ return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, no_obstacle);
+ }
else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ {
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle);
+ }
}
/*
@@ -179,6 +223,10 @@ move__IDLE__start (void)
* post an event for the top FSM to tell we have finished
* position_intermediary => MOVING_TO_INTERMEDIATE_POSITION
* go to the next intermediate position computed by the path module
+ * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * store current moving direction
+ * stop the bot
+ * post an event for the top FSM to be waked up later
*/
fsm_branch_t
move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void)
@@ -193,22 +241,40 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void)
else
{
/* Get next position */
- move_get_next_position (&move_data.intermediate);
- /* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ if (move_get_next_position (&move_data.intermediate))
+ {
+ /* Go to the next intermediate position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ else
+ asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary);
+ }
else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
- return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary);
+ {
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
+ /* Stop the bot */
+ asserv_stop_motor ();
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, no_intermediate_path_found);
+ }
}
}
/*
* MOVING_TO_INTERMEDIATE_POSITION =bot_move_obstacle=>
- * => MOVING_TO_INTERMEDIATE_POSITION
+ * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
* compute the obstacle position
- * give the obstacle position to the path module
- * go to the next intermediate position computed by the path module
+ * get next intermediate position from path module
+ * go to next intermediate position
+ * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * compute the obstacle position
+ * get next intermediate position from path module failed
+ * store current moving direction
+ * stop the bot
+ * post an event for the top FSM to be waked up later
*/
fsm_branch_t
move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void)
@@ -216,13 +282,25 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void)
/* Compute obstacle position */
move_obstacle_here ();
/* Get next position */
- move_get_next_position (&move_data.intermediate);
- /* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ if (move_get_next_position (&move_data.intermediate))
+ {
+ /* Go to the next intermediate position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ else
+ asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found);
+ }
else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
- return move_next (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle);
+ {
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
+ /* Stop the bot */
+ asserv_stop_motor ();
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, no_intermediate_path_found);
+ }
}
/*
@@ -243,39 +321,63 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_failed (void)
/*
* MOVING_BACKWARD =bot_move_failed=>
- * => MOVING_TO_INTERMEDIATE_POSITION
- * do the same as when we succeed
- * give the obstacle position to the path module
- * give the current position of the bot to the path module
- * get the intermediate position from path module
+ * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
+ * get next intermediate position from path module
+ * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * get next intermediate position from path module, failed
+ * stop the bot
+ * post an event for the top FSM to be waked up later
*/
fsm_branch_t
move__MOVING_BACKWARD__bot_move_failed (void)
{
/* Call generic function */
- move_after_moving_backward ();
- return move_next (MOVING_BACKWARD, bot_move_failed);
+ if (move_after_moving_backward ())
+ {
+ return move_next_branch (MOVING_BACKWARD, bot_move_failed, intermediate_path_found);
+ }
+ else
+ {
+ /* Stop the bot */
+ asserv_stop_motor ();
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (MOVING_BACKWARD, bot_move_failed, no_intermediate_path_found);
+ }
}
/*
* MOVING_BACKWARD =bot_move_succeed=>
- * => MOVING_TO_INTERMEDIATE_POSITION
- * give the obstacle position to the path module
- * give the current position of the bot to the path module
- * get the intermediate position from path module
+ * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
+ * get next intermediate position from path module
+ * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * get next intermediate position from path module, failed
+ * stop the bot
+ * post an event for the top FSM to be waked up later
*/
fsm_branch_t
move__MOVING_BACKWARD__bot_move_succeed (void)
{
/* Call generic function */
- move_after_moving_backward ();
- return move_next (MOVING_BACKWARD, bot_move_succeed);
+ if (move_after_moving_backward ())
+ {
+ return move_next_branch (MOVING_BACKWARD, bot_move_succeed, intermediate_path_found);
+ }
+ else
+ {
+ /* Stop the bot */
+ asserv_stop_motor ();
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (MOVING_BACKWARD, bot_move_succeed, no_intermediate_path_found);
+ }
}
/*
* MOVING_TO_FINAL_POSITION =bot_move_failed=>
* => MOVING_BACKWARD
- * store the current position of the obstacle
+ * compute the obstacle position
+ * store current moving direction (for possible failed of path module)
* move backward to turn freely
*/
fsm_branch_t
@@ -283,6 +385,8 @@ move__MOVING_TO_FINAL_POSITION__bot_move_failed (void)
{
/* Compute obstacle position */
move_obstacle_here ();
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
/* Move backward to turn freely */
asserv_move_linearly (-PG_MOVE_DISTANCE);
return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed);
@@ -303,11 +407,16 @@ move__MOVING_TO_FINAL_POSITION__bot_move_succeed (void)
/*
* MOVING_TO_FINAL_POSITION =bot_move_obstacle=>
- * => MOVING_TO_INTERMEDIATE_POSITION
+ * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
+ * compute the obstacle position
+ * get next intermediate position from path module
+ * go to next intermediate position
+ * no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
* compute the obstacle position
- * give the obstacle position to the path module
- * give the current position of the bot to the path module
- * get the intermediate position from path module
+ * get next intermediate position from path module failed
+ * store current moving direction
+ * stop the bot
+ * post an event for the top FSM to be waked up later
*/
fsm_branch_t
move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void)
@@ -315,12 +424,24 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void)
/* Compute obstacle position */
move_obstacle_here ();
/* Get next position */
- move_get_next_position (&move_data.intermediate);
- /* Go to the next intermediate position */
- if (move_data.backward_movement_allowed)
- asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ if (move_get_next_position (&move_data.intermediate))
+ {
+ /* Go to the next intermediate position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ else
+ asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found);
+ }
else
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
- return move_next (MOVING_TO_FINAL_POSITION, bot_move_obstacle);
+ {
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
+ /* Stop the bot */
+ asserv_stop_motor ();
+ /* Post an event for the top FSM to be waked up later */
+ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
+ return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, no_intermediate_path_found);
+ }
}