From 2376aeb614d69d7050209f858818bfa7d36be398 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 2 May 2011 23:32:51 +0200 Subject: digital/io: adapt FSM to new FSM system (AngFSM) --- digital/io/src/hola.c | 115 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 115 insertions(+) (limited to 'digital/io/src/hola.c') diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c index 1fd15f18..c7d07e21 100644 --- a/digital/io/src/hola.c +++ b/digital/io/src/hola.c @@ -23,3 +23,118 @@ * * }}} */ +#define FSM_NAME AI + +#include "common.h" +#include "fsm.h" +#include "asserv.h" +#include "mimot.h" +#include "bot.h" +#include "playground.h" + +#include "modules/math/fixed/fixed.h" + +uint32_t hola_angle; +int16_t hola_timeout; + +FSM_STATES ( + /* waiting start */ + HOLA_IDLE, + /* wait hola signal */ + HOLA_WAIT_HOLA, + /* wait until the jack is inserted for the first time */ + HOLA_WAIT_JACK_IN, + /* wait until the jack is removed to initialise robot */ + HOLA_WAIT_JACK_OUT, + /* moving clamps in rounds */ + HOLA_ROUNDS, + /* moving clamp up */ + HOLA_UP, + HOLA_CENTER, + HOLA_CLAP_CLOSE, + HOLA_CLAP_OPEN) + +FSM_EVENTS (hola_start) + +FSM_START_WITH (HOLA_IDLE) + +FSM_TRANS (HOLA_IDLE, start, HOLA_WAIT_HOLA) +{ + return FSM_NEXT (HOLA_IDLE, start); +} + +FSM_TRANS (HOLA_WAIT_HOLA, hola_start, HOLA_WAIT_JACK_IN) +{ + return FSM_NEXT (HOLA_WAIT_HOLA, hola_start); +} + +FSM_TRANS (HOLA_WAIT_JACK_IN, jack_inserted_into_bot, HOLA_WAIT_JACK_OUT) +{ + return FSM_NEXT (HOLA_WAIT_JACK_IN, jack_inserted_into_bot); +} + +FSM_TRANS (HOLA_WAIT_JACK_OUT, jack_removed_from_bot, HOLA_ROUNDS) +{ + return FSM_NEXT (HOLA_WAIT_JACK_OUT, jack_removed_from_bot); +} + +FSM_TRANS (HOLA_ROUNDS, asserv_last_cmd_ack, + /* move clamp rounds */ + no_timeout, HOLA_ROUNDS, + /* move up, rotate */ + timeout, HOLA_UP) +{ + if (++hola_timeout < 150) + { + int32_t cos = fixed_cos_f824 (hola_angle) + 0x1000000; + int32_t sin = fixed_sin_f824 (hola_angle) + 0x1000000; + int32_t dx = BOT_CLAMP_OPEN_STEP + + fixed_mul_f824 (BOT_CLAMP_STROKE_STEP / 3 / 2, cos); + int32_t dy = BOT_ELEVATOR_ZERO_SPEED + + fixed_mul_f824 (BOT_ELEVATOR_REST_STEP / 3 / 2, sin); + mimot_move_motor0_absolute (dx, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (dx, BOT_CLAMP_SPEED); + asserv_move_motor0_absolute (dy, BOT_ELEVATOR_SPEED); + hola_angle += 0x1000000 / (225 / 12); + return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, no_timeout); + } + else + { + asserv_set_speed (0x10, 0x1c, 0x10, 0x1c); + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED / 3); + asserv_move_angularly (POSITION_A_DEG (174)); + return FSM_NEXT (HOLA_ROUNDS, asserv_last_cmd_ack, timeout); + } +} + +FSM_TRANS (HOLA_UP, bot_move_succeed, HOLA_CENTER) +{ + asserv_set_speed (0x10, 0x20, 0x10, 0x20); + asserv_move_angularly (POSITION_A_DEG (-90)); + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP / 2, BOT_ELEVATOR_SPEED); + return FSM_NEXT (HOLA_UP, bot_move_succeed); +} + +FSM_TRANS (HOLA_CENTER, bot_move_succeed, HOLA_CLAP_CLOSE) +{ + mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + return FSM_NEXT (HOLA_CENTER, bot_move_succeed); +} + +FSM_TRANS (HOLA_CLAP_CLOSE, clamp_succeed, HOLA_CLAP_OPEN) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return FSM_NEXT (HOLA_CLAP_CLOSE, clamp_succeed); +} + +/* + * close clamp + */ +FSM_TRANS (HOLA_CLAP_OPEN, clamp_succeed, HOLA_CLAP_CLOSE) +{ + mimot_motor0_clamp (BOT_CLAMP_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_SPEED, 0); + return FSM_NEXT (HOLA_CLAP_OPEN, clamp_succeed); +} -- cgit v1.2.3