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 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/bot.h | 6 +++++ digital/io/src/contact.c | 3 ++- digital/io/src/contact.h | 3 ++- digital/io/src/loader.fsm | 22 +++++++++++++++++++ digital/io/src/main.c | 4 ++++ 6 files changed, 87 insertions(+), 2 deletions(-) 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); +} + diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index 39467252..e5f280bd 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -62,6 +62,9 @@ /** Elevator stroke in steps. */ #define BOT_ELEVATOR_STROKE_STEP 7089 +/** Elevator unloading up position in steps. */ +#define BOT_ELEVATOR_UNLOAD_STEP 5600 + /** Elevator resting up position in steps. */ #define BOT_ELEVATOR_REST_STEP 4111 @@ -77,6 +80,9 @@ /** Clamp stroke in steps. */ #define BOT_CLAMP_STROKE_STEP 5627 +/** Clamp open position in steps. */ +#define BOT_CLAMP_OPEN_STEP 30 + /** Clamp work speed. */ #define BOT_CLAMP_SPEED 0x60 diff --git a/digital/io/src/contact.c b/digital/io/src/contact.c index 4483fef3..8bcb8910 100644 --- a/digital/io/src/contact.c +++ b/digital/io/src/contact.c @@ -30,5 +30,6 @@ void contact_init (void) { /* Pull-up. */ - //IO_SET (CONTACT_EXAMPLE); + IO_SET (CONTACT_BUMPER0); + IO_SET (CONTACT_BUMPER1); } diff --git a/digital/io/src/contact.h b/digital/io/src/contact.h index e50a0833..4fcfa05c 100644 --- a/digital/io/src/contact.h +++ b/digital/io/src/contact.h @@ -26,7 +26,8 @@ * }}} */ #include "simu.host.h" -#define CONTACT_EXAMPLE C, 0 +#define CONTACT_BUMPER0 C, 0 +#define CONTACT_BUMPER1 C, 1 /** * Initialize the contact module. diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 05918468..747bb51e 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -27,6 +27,12 @@ States: moving to the down position LOADER_ERROR error while moving up, stop in an unknown state + LOADER_LOAD_CLOSING + close clamp + LOADER_LOAD_UPING + move load up to the unload position + LOADER_LOAD_UNLOADING + open clamp to unload Events: elevator_succeed @@ -35,6 +41,8 @@ Events: asserv failure result clamp_succeed asserv success result, clamp movement cannot fail + loader_element + element sensed between clamp to be loaded loader_down make the loader ready to load an element loader_up @@ -80,6 +88,8 @@ LOADER_UP: LOADER_DOWN: loader_up -> LOADER_UPING move up + loader_element -> LOADER_LOAD_CLOSING + close clamp LOADER_ERROR: loader_down -> LOADER_DOWNING @@ -99,3 +109,15 @@ LOADER_UPING: elevator_failed -> LOADER_ERROR post loader_errored event +LOADER_LOAD_CLOSING: + clamp_succeed -> LOADER_LOAD_UPING + move up + +LOADER_LOAD_UPING: + elevator_succeed -> LOADER_LOAD_UNLOADING + open clamp + +LOADER_LOAD_UNLOADING: + clamp_succeed -> LOADER_DOWNING + move down + diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 21bfdff8..3a5e5901 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -152,6 +152,10 @@ main_event_to_fsm (void) if (motorm0_status == success && motorm1_status == success) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); + /* Contacts. */ + if (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element); + /* Jack */ if (switch_get_jack ()) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot); -- cgit v1.2.3