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 +++++++++++++++++++++++++++++++++++++++++- digital/io/src/ai_top_cb.c | 4 +- digital/io/src/bot.h | 6 +++ digital/io/src/loader.c | 14 ++++++ digital/io/src/loader.fsm | 48 +++++++++++++++++++- digital/io/src/loader.h | 8 ++++ 6 files changed, 177 insertions(+), 4 deletions(-) (limited to 'digital') 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); +} + diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 882fb84f..4f349ead 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -31,7 +31,7 @@ #include "move.h" #include "chrono.h" #include "playground.h" -#include "asserv.h" +#include "loader.h" /** * Time limit. @@ -73,6 +73,7 @@ ai__GO_FAR__move_fsm_succeed (void) { position_t pos = PG_POSITION_DEG (2625, 253, -90); move_start (pos, 0); + loader_down (); return ai_next (GO_FAR, move_fsm_succeed); } @@ -98,6 +99,7 @@ ai__GO_NEAR__move_fsm_succeed (void) { position_t pos = PG_POSITION_DEG (825, 1253, -29); move_start (pos, 0); + loader_up (); return ai_next (GO_NEAR, move_fsm_succeed); } diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index aa889970..39467252 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -62,6 +62,12 @@ /** Elevator stroke in steps. */ #define BOT_ELEVATOR_STROKE_STEP 7089 +/** Elevator resting up position in steps. */ +#define BOT_ELEVATOR_REST_STEP 4111 + +/** Elevator down position in steps. */ +#define BOT_ELEVATOR_DOWN_STEP 30 + /** Elevator work speed. */ #define BOT_ELEVATOR_SPEED 0x60 diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index bf297161..019ec41b 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -25,3 +25,17 @@ #include "common.h" #include "loader.h" +#include "fsm.h" + +void +loader_up (void) +{ + fsm_handle_event (&ai_fsm, AI_EVENT_loader_up); +} + +void +loader_down (void) +{ + fsm_handle_event (&ai_fsm, AI_EVENT_loader_down); +} + diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 554ff08a..05918468 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -17,6 +17,16 @@ States: find clamp zero position LOADER_INIT_ELEVATOR_UP move the elevator up so that the robot can go to the wall + LOADER_UP + up position, clamp is protected + LOADER_DOWN + down position, ready to take an element + LOADER_UPING + moving to the up position + LOADER_DOWNING + moving to the down position + LOADER_ERROR + error while moving up, stop in an unknown state Events: elevator_succeed @@ -25,6 +35,16 @@ Events: asserv failure result clamp_succeed asserv success result, clamp movement cannot fail + loader_down + make the loader ready to load an element + loader_up + protect the clamp by moving it up + loader_downed + posted when loader successfully moved down + loader_uped + posted when loader successfully moved up + loader_errored + posted when loader in an unknown state due to error while moving up LOADER_IDLE: start -> LOADER_WAIT_JACK_IN @@ -49,7 +69,33 @@ LOADER_INIT_CLAMP_ZERO: clamp_succeed -> LOADER_INIT_ELEVATOR_UP LOADER_INIT_ELEVATOR_UP: - elevator_succeed -> LOADER_IDLE + elevator_succeed -> LOADER_UP elevator_failed -> LOADER_IDLE initialisation failure +LOADER_UP: + loader_down -> LOADER_DOWNING + move down + +LOADER_DOWN: + loader_up -> LOADER_UPING + move up + +LOADER_ERROR: + loader_down -> LOADER_DOWNING + move down + loader_up -> LOADER_UPING + move up + +LOADER_DOWNING: + elevator_succeed -> LOADER_DOWN + post loader_downed event + elevator_failed -> LOADER_UPING + something is blocking, move it up + +LOADER_UPING: + elevator_succeed -> LOADER_UP + post loader_uped event + elevator_failed -> LOADER_ERROR + post loader_errored event + diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h index 897763ab..6ca7459f 100644 --- a/digital/io/src/loader.h +++ b/digital/io/src/loader.h @@ -25,4 +25,12 @@ * * }}} */ +/** Move loader up, in a rest position. */ +void +loader_up (void); + +/** Move loader down, ready to take elements. */ +void +loader_down (void); + #endif /* loader_h */ -- cgit v1.2.3