From 7c593a7f73a4d56a7eb1e6727a2f5b2007133a27 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:25:16 +0200 Subject: digital/io/src: add up and down clamp positions --- digital/io/src/ai_loader_cb.c | 101 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 99 insertions(+), 2 deletions(-) (limited to 'digital/io/src/ai_loader_cb.c') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 1d4f816e..10e32211 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -28,6 +28,7 @@ #include "asserv.h" #include "mimot.h" #include "bot.h" +#include "main.h" /* * LOADER_IDLE =start=> @@ -85,7 +86,7 @@ ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void) fsm_branch_t ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void) { - asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); @@ -104,7 +105,7 @@ ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) /* * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> - * => LOADER_IDLE + * => LOADER_UP */ fsm_branch_t ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) @@ -126,3 +127,99 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); } +/* + * LOADER_UP =loader_down=> + * => LOADER_DOWNING + * move down + */ +fsm_branch_t +ai__LOADER_UP__loader_down (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_UP, loader_down); +} + +/* + * LOADER_DOWN =loader_up=> + * => LOADER_UPING + * move up + */ +fsm_branch_t +ai__LOADER_DOWN__loader_up (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_DOWN, loader_up); +} + +/* + * LOADER_UPING =elevator_succeed=> + * => LOADER_UP + * post loader_uped event + */ +fsm_branch_t +ai__LOADER_UPING__elevator_succeed (void) +{ + main_post_event_for_top_fsm = AI_EVENT_loader_uped; + return ai_next (LOADER_UPING, elevator_succeed); +} + +/* + * LOADER_UPING =elevator_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_UPING__elevator_failed (void) +{ + main_post_event_for_top_fsm = AI_EVENT_loader_errored; + return ai_next (LOADER_UPING, elevator_failed); +} + +/* + * LOADER_DOWNING =elevator_succeed=> + * => LOADER_DOWN + * post loader_downed event + */ +fsm_branch_t +ai__LOADER_DOWNING__elevator_succeed (void) +{ + main_post_event_for_top_fsm = AI_EVENT_loader_downed; + return ai_next (LOADER_DOWNING, elevator_succeed); +} + +/* + * LOADER_DOWNING =elevator_failed=> + * => LOADER_UPING + * something is blocking, move it up + */ +fsm_branch_t +ai__LOADER_DOWNING__elevator_failed (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_DOWNING, elevator_failed); +} + +/* + * LOADER_ERROR =loader_down=> + * => LOADER_DOWNING + * move down + */ +fsm_branch_t +ai__LOADER_ERROR__loader_down (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_ERROR, loader_down); +} + +/* + * LOADER_ERROR =loader_up=> + * => LOADER_UPING + * move up + */ +fsm_branch_t +ai__LOADER_ERROR__loader_up (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_ERROR, loader_up); +} + -- cgit v1.2.3