From c17d0d5431258c7c5a006ddeffbb0dad1592528f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 12 May 2010 20:38:15 +0200 Subject: digital/io/src: add gate init --- digital/io/src/ai_loader_cb.c | 50 +++++++++++++++++++++++++++++++++++++++++-- digital/io/src/bot.h | 6 ++++++ digital/io/src/loader.fsm | 31 +++++++++++++++++++++++++-- digital/io/src/main.c | 5 +++++ 4 files changed, 88 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 7f1a6073..3a2782ab 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -55,6 +55,7 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) * => LOADER_INIT_ELEVATOR_ZERO * find elevator zero * close clamp + * find gate zero */ fsm_branch_t ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) @@ -62,6 +63,7 @@ ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED); mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + asserv_motor1_zero_position (BOT_GATE_SPEED); return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot); } @@ -93,7 +95,7 @@ ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void) /* * LOADER_INIT_CLAMP_ZERO =clamp_succeed=> - * => LOADER_INIT_ELEVATOR_UP + * => LOADER_INIT_GATE_ZERO */ fsm_branch_t ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) @@ -101,13 +103,25 @@ ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void) return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed); } +/* + * LOADER_INIT_GATE_ZERO =gate_succeed=> + * => LOADER_INIT_ELEVATOR_UP + */ +fsm_branch_t +ai__LOADER_INIT_GATE_ZERO__gate_succeed (void) +{ + return ai_next (LOADER_INIT_GATE_ZERO, gate_succeed); +} + /* * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> - * => LOADER_UP + * => LOADER_INIT_GATE_OPEN + * open gate */ fsm_branch_t ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) { + asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed); } @@ -125,6 +139,38 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); } +/* + * LOADER_INIT_GATE_OPEN =gate_succeed=> + * => LOADER_INIT_GATE_WAIT + */ +fsm_branch_t +ai__LOADER_INIT_GATE_OPEN__gate_succeed (void) +{ + return ai_next (LOADER_INIT_GATE_OPEN, gate_succeed); +} + +/* + * LOADER_INIT_GATE_WAIT =state_timeout=> + * => LOADER_INIT_GATE_CLOSE + * close gate + */ +fsm_branch_t +ai__LOADER_INIT_GATE_WAIT__state_timeout (void) +{ + asserv_move_motor1_absolute (0, BOT_GATE_SPEED); + return ai_next (LOADER_INIT_GATE_WAIT, state_timeout); +} + +/* + * LOADER_INIT_GATE_CLOSE =gate_succeed=> + * => LOADER_UP + */ +fsm_branch_t +ai__LOADER_INIT_GATE_CLOSE__gate_succeed (void) +{ + return ai_next (LOADER_INIT_GATE_CLOSE, gate_succeed); +} + /* * LOADER_UP =loader_down=> * => LOADER_DOWNING diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index df99aacb..f0264240 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -110,6 +110,12 @@ /** Clamp open loop PWM. */ #define BOT_CLAMP_PWM 0x100 +/** Gate work speed. */ +#define BOT_GATE_SPEED 0x46 + +/** Gate stroke in steps. */ +#define BOT_GATE_STROKE_STEP 0x2081 + /** * Definition of the colors. */ diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 98b18da2..1978ef41 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -15,8 +15,16 @@ States: test clamp closing LOADER_INIT_CLAMP_ZERO find clamp zero position + LOADER_INIT_GATE_ZERO + find gate zero LOADER_INIT_ELEVATOR_UP move the elevator up so that the robot can go to the wall + LOADER_INIT_GATE_OPEN + open gate to unload elements before start + LOADER_INIT_GATE_WAIT[timeout=225] + wait with gate open + LOADER_INIT_GATE_CLOSE + close gate before start LOADER_UP up position, clamp is protected LOADER_DOWN @@ -47,6 +55,10 @@ Events: elevator above unloading position clamp_succeed asserv success result, clamp movement cannot fail + gate_succeed + asserv success result + gate_failed + asserv failure result loader_element element sensed between clamp to be loaded loader_down @@ -70,6 +82,7 @@ LOADER_WAIT_JACK_OUT: jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO find elevator zero close clamp + find gate zero LOADER_INIT_ELEVATOR_ZERO: elevator_succeed -> LOADER_INIT_CLAMP_CLOSE @@ -80,13 +93,27 @@ LOADER_INIT_CLAMP_CLOSE: find clamp zero LOADER_INIT_CLAMP_ZERO: - clamp_succeed -> LOADER_INIT_ELEVATOR_UP + clamp_succeed -> LOADER_INIT_GATE_ZERO + +LOADER_INIT_GATE_ZERO: + gate_succeed -> LOADER_INIT_ELEVATOR_UP LOADER_INIT_ELEVATOR_UP: - elevator_succeed -> LOADER_UP + elevator_succeed -> LOADER_INIT_GATE_OPEN + open gate elevator_failed -> LOADER_IDLE initialisation failure +LOADER_INIT_GATE_OPEN: + gate_succeed -> LOADER_INIT_GATE_WAIT + +LOADER_INIT_GATE_WAIT: + state_timeout -> LOADER_INIT_GATE_CLOSE + close gate + +LOADER_INIT_GATE_CLOSE: + gate_succeed -> LOADER_UP + LOADER_UP: loader_down -> LOADER_DOWNING move down diff --git a/digital/io/src/main.c b/digital/io/src/main.c index f33afbb5..f05653eb 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -150,6 +150,11 @@ main_event_to_fsm (void) else if (motor0_status == failure) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed); + if (motor1_status == success) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_succeed); + else if (motor1_status == failure) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_gate_failed); + if (motorm0_status == success && motorm1_status == success) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); -- cgit v1.2.3