summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/move.c4
-rw-r--r--digital/io/src/move.h6
-rw-r--r--digital/io/src/move_cb.c28
-rw-r--r--digital/io/src/top_cb.c19
4 files changed, 41 insertions, 16 deletions
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 2c0a0c5e..51c22e87 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -34,11 +34,13 @@ struct move_data_t move_data;
/* Go to a position with the start FSM. */
void
-move_start (uint16_t position_x, uint16_t position_y)
+move_start (uint16_t position_x, uint16_t position_y,
+ uint8_t backward_movement_allowed)
{
/* Set parameters. */
move_data.final.x = position_x;
move_data.final.y = position_y;
+ move_data.backward_movement_allowed = backward_movement_allowed;
/* Start the FSM. */
fsm_init (&move_fsm);
fsm_handle_event (&move_fsm, MOVE_EVENT_start);
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index dc2c2603..5a328950 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -47,6 +47,8 @@ struct move_data_t
move_position_t intermediate;
/** Obstacle position. */
move_position_t obstacle;
+ /** Backward direction allowed flag. */
+ uint8_t backward_movement_allowed;
};
/**
@@ -58,8 +60,10 @@ extern struct move_data_t move_data;
* Go to a position with the start FSM.
* @param position_x the X position.
* @param position_y the Y position.
+ * @param backward_movement_allowed do we allow backward movement?
*/
void
-move_start (uint16_t position_x, uint16_t position_y);
+move_start (uint16_t position_x, uint16_t position_y, uint8_t
+ backward_movement_allowed);
#endif /* move_h */
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index a92135ea..7254a504 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -109,6 +109,9 @@ move_compute_obstacle_position (asserv_position_t cur,
{
/* Convert the angle */
uint32_t angle = cur.a;
+ /* Change angle when going backward */
+ if (move_data.backward_movement_allowed)
+ angle += 0x8000;
angle = angle << 8;
/* X */
obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle),
@@ -142,7 +145,10 @@ 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 */
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ 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);
}
/*
@@ -154,7 +160,10 @@ fsm_branch_t
move__IDLE__start (void)
{
/* Go to the destination position */
- asserv_goto (move_data.final.x, move_data.final.y);
+ 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 (IDLE, start);
}
@@ -180,7 +189,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void)
/* Get next position */
move_get_next_position (&move_data.intermediate);
/* Go to the next intermediate position */
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ 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);
}
}
@@ -200,7 +212,10 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void)
/* Get next position */
move_get_next_position (&move_data.intermediate);
/* Go to the next intermediate position */
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ 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 (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle);
}
@@ -296,7 +311,10 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void)
/* Get next position */
move_get_next_position (&move_data.intermediate);
/* Go to the next intermediate position */
- asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ 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 (MOVING_TO_FINAL_POSITION, bot_move_obstacle);
}
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index 55b1077f..b8bf6a62 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -67,7 +67,7 @@ top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void)
asserv_get_position (&position);
/* FIXME: linear move is better? */
/* Move away from the gutter */
- move_start (position.x, position.y + BOT_MIN_DISTANCE_TURN_FREE);
+ move_start (position.x, position.y + BOT_MIN_DISTANCE_TURN_FREE, 0);
return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished);
}
@@ -102,7 +102,7 @@ fsm_branch_t
top__MOVE_AWAY_FROM_START_BORDER__move_fsm_finished (void)
{
/* Start the move FSM to our samples distributor */
- move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
+ move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y, 0);
return top_next (MOVE_AWAY_FROM_START_BORDER, move_fsm_finished);
}
@@ -116,7 +116,7 @@ fsm_branch_t
top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void)
{
/* Start the move FSM to our ice distributor */
- move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y);
+ move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y, 0);
return top_next (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished);
}
@@ -230,13 +230,13 @@ top__GET_ICE_FROM_OUR_ICE_DISTRIBUTOR__get_samples_fsm_finished (void)
if (0)
{
/* Start the move FSM to the adverse ice distributor */
- move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y);
+ move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0);
return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, not_full);
}
else
{
/* Start the move FSM to go to the gutter */
- move_start (PG_GUTTER_X, PG_GUTTER_Y);
+ move_start (PG_GUTTER_X, PG_GUTTER_Y, 1);
return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, full);
}
}
@@ -261,14 +261,15 @@ top__MOVE_AWAY_FROM_GUTTER_BORDER__move_fsm_finished (void)
/* Two first loops, normal behaviour (our sample distributor, them
* ice) */
/* Start the move FSM */
- move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
+ move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y,
+ 0);
return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, first_loop);
}
else
{
/* Second loops, ice only */
/* Start the move FSM */
- move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y);
+ move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0);
return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, second_loop);
}
}
@@ -283,7 +284,7 @@ fsm_branch_t
top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void)
{
/* Start the move FSM to go to the gutter */
- move_start (PG_GUTTER_X, PG_GUTTER_Y);
+ move_start (PG_GUTTER_X, PG_GUTTER_Y, 1);
return top_next (GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR, get_samples_fsm_finished);
}
@@ -312,7 +313,7 @@ top__CONFIGURE_ASSERV__settings_acknowledged (void)
/* Clear the flag for the setting acknowleged */
top_waiting_for_settings_ack_ = 0;
/* Start the move FSM to move the the bot away from the border */
- move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE);
+ move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE, 0);
return top_next (CONFIGURE_ASSERV, settings_acknowledged);
}