From 6efb5213ba0113e92f168156c80cd748aea53745 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 4 May 2010 02:25:27 +0200 Subject: digital/io/src: first element pickup --- digital/io/src/ai_loader_cb.c | 51 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) (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 10e32211..f759b417 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -151,6 +151,19 @@ ai__LOADER_DOWN__loader_up (void) return ai_next (LOADER_DOWN, loader_up); } +/* + * LOADER_DOWN =loader_element=> + * => LOADER_LOAD_CLOSING + * close clamp + */ +fsm_branch_t +ai__LOADER_DOWN__loader_element (void) +{ + mimot_move_motor0_absolute (BOT_CLAMP_STROKE_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_STROKE_STEP, BOT_CLAMP_SPEED); + return ai_next (LOADER_DOWN, loader_element); +} + /* * LOADER_UPING =elevator_succeed=> * => LOADER_UP @@ -223,3 +236,41 @@ ai__LOADER_ERROR__loader_up (void) return ai_next (LOADER_ERROR, loader_up); } +/* + * LOADER_LOAD_CLOSING =clamp_succeed=> + * => LOADER_LOAD_UPING + * move up + */ +fsm_branch_t +ai__LOADER_LOAD_CLOSING__clamp_succeed (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP, + BOT_ELEVATOR_SPEED); + return ai_next (LOADER_LOAD_CLOSING, clamp_succeed); +} + +/* + * LOADER_LOAD_UPING =elevator_succeed=> + * => LOADER_LOAD_UNLOADING + * open clamp + */ +fsm_branch_t +ai__LOADER_LOAD_UPING__elevator_succeed (void) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return ai_next (LOADER_LOAD_UPING, elevator_succeed); +} + +/* + * LOADER_LOAD_UNLOADING =clamp_succeed=> + * => LOADER_DOWNING + * move down + */ +fsm_branch_t +ai__LOADER_LOAD_UNLOADING__clamp_succeed (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_LOAD_UNLOADING, clamp_succeed); +} + -- cgit v1.2.3