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 ++++++++++++++++++++++++++++++++--- digital/io/src/bot.h | 9 +++++++++ digital/io/src/loader.fsm | 16 +++++++++++++++- digital/io/src/main.c | 11 ++++++++++- 4 files changed, 66 insertions(+), 5 deletions(-) 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); } /* diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index fa8ad845..634d68e7 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -68,6 +68,15 @@ /** Elevator find zero speed. */ #define BOT_ELEVATOR_ZERO_SPEED 0x10 +/** Clamp stroke in steps. */ +#define BOT_CLAMP_STROKE_STEP 13500 + +/** Clamp work speed. */ +#define BOT_CLAMP_SPEED 0x40 + +/** Clamp find zero speed. */ +#define BOT_CLAMP_ZERO_SPEED 0x10 + /** * Definition of the colors. */ diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index c89e8931..554ff08a 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -11,6 +11,10 @@ States: wait until the jack is removed to initialise robot LOADER_INIT_ELEVATOR_ZERO move the elevator down to find zero position + LOADER_INIT_CLAMP_CLOSE + test clamp closing + LOADER_INIT_CLAMP_ZERO + find clamp zero position LOADER_INIT_ELEVATOR_UP move the elevator up so that the robot can go to the wall @@ -19,6 +23,8 @@ Events: asserv success result elevator_failed asserv failure result + clamp_succeed + asserv success result, clamp movement cannot fail LOADER_IDLE: start -> LOADER_WAIT_JACK_IN @@ -29,10 +35,18 @@ LOADER_WAIT_JACK_IN: LOADER_WAIT_JACK_OUT: jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO find elevator zero + close clamp LOADER_INIT_ELEVATOR_ZERO: - elevator_succeed -> LOADER_INIT_ELEVATOR_UP + elevator_succeed -> LOADER_INIT_CLAMP_CLOSE + +LOADER_INIT_CLAMP_CLOSE: + clamp_succeed -> LOADER_INIT_CLAMP_ZERO move elevator up + find clamp zero + +LOADER_INIT_CLAMP_ZERO: + clamp_succeed -> LOADER_INIT_ELEVATOR_UP LOADER_INIT_ELEVATOR_UP: elevator_succeed -> LOADER_IDLE diff --git a/digital/io/src/main.c b/digital/io/src/main.c index b0973d66..21bfdff8 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -41,6 +41,7 @@ #include "simu.host.h" #include "asserv.h" +#include "mimot.h" #include "twi_master.h" #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ #include "fsm.h" /* fsm_* */ @@ -126,12 +127,16 @@ main_event_to_fsm (void) asserv_status_e move_status = none, motor0_status = none, - motor1_status = none; + motor1_status = none, + motorm0_status = none, + motorm1_status = none; /* Get status of move, motor0 and motor1. */ move_status = asserv_move_cmd_status (); motor0_status = asserv_motor0_cmd_status (); motor1_status = asserv_motor1_cmd_status (); + motorm0_status = mimot_motor0_cmd_status (); + motorm1_status = mimot_motor1_cmd_status (); /* Check commands move status. */ if (move_status == success) @@ -144,6 +149,9 @@ main_event_to_fsm (void) else if (motor0_status == failure) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed); + if (motorm0_status == success && motorm1_status == success) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); + /* Jack */ if (switch_get_jack ()) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot); @@ -195,6 +203,7 @@ main_init (void) utils_delay_ms (500); /* Asserv communication */ asserv_init (); + mimot_init (); /* TWI master. */ twi_master_init (); /* Switch module */ -- cgit v1.2.3