From 8bb3210e139183c36984427926bbf0061dc5e6dd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:25:05 +0200 Subject: digital/io/src: add clamp to loader FSM --- digital/io/src/ai_loader_cb.c | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 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 18cdc4cb..1d4f816e 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -26,6 +26,7 @@ #include "fsm.h" #include "ai_cb.h" #include "asserv.h" +#include "mimot.h" #include "bot.h" /* @@ -52,25 +53,53 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=> * => LOADER_INIT_ELEVATOR_ZERO * find elevator zero + * close clamp */ fsm_branch_t ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) { asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); + mimot_move_motor0_absolute (BOT_CLAMP_STROKE_STEP / 4, + BOT_CLAMP_ZERO_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_STROKE_STEP / 4, + BOT_CLAMP_ZERO_SPEED); return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); } /* * LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=> - * => LOADER_INIT_ELEVATOR_UP - * move elevator up + * => LOADER_INIT_CLAMP_CLOSE */ fsm_branch_t ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void) +{ + return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); +} + +/* + * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=> + * => LOADER_INIT_CLAMP_ZERO + * move elevator up + * find clamp zero + */ +fsm_branch_t +ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void) { asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); + mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED); + mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED); + return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed); +} + +/* + * LOADER_INIT_CLAMP_ZERO =clamp_succeed=> + * => LOADER_INIT_ELEVATOR_UP + */ +fsm_branch_t +ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) +{ + return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed); } /* -- cgit v1.2.3