From 7f4655ebc5807a23782fc3efc1a722217a0081d4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 May 2010 23:02:00 +0200 Subject: digital/io/src: add hola code --- digital/io/src/Makefile | 2 +- digital/io/src/ai.conf | 1 + digital/io/src/ai_hola_cb.c | 165 +++++++++++++++++++++ digital/io/src/ai_loader_cb.c | 10 ++ digital/io/src/fsm.h | 2 +- .../io/src/fsm_templates/template_hola_cb_skel.c | 11 ++ digital/io/src/hola.c | 25 ++++ digital/io/src/hola.fsm | 59 ++++++++ digital/io/src/loader.fsm | 3 +- digital/io/src/main.c | 7 +- digital/io/src/top.fsm | 3 + 11 files changed, 283 insertions(+), 5 deletions(-) create mode 100644 digital/io/src/ai_hola_cb.c create mode 100644 digital/io/src/fsm_templates/template_hola_cb_skel.c create mode 100644 digital/io/src/hola.c create mode 100644 digital/io/src/hola.fsm (limited to 'digital/io') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index de73dfe0..5dfea34b 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -21,7 +21,7 @@ HOST_LIBS = -lm # FSM. FSM = ai -FSM_SOURCES := top init move loader +FSM_SOURCES := top init move loader hola # Include FSM makefile. include $(BASE)/make/Makefile.fsm diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf index b6527759..ce7f6374 100644 --- a/digital/io/src/ai.conf +++ b/digital/io/src/ai.conf @@ -9,3 +9,4 @@ template_top_cb_skel.c = %_top_cb_skel.c template_init_cb_skel.c = %_init_cb_skel.c template_move_cb_skel.c = %_move_cb_skel.c template_loader_cb_skel.c = %_loader_cb_skel.c +template_hola_cb_skel.c = %_hola_cb_skel.c diff --git a/digital/io/src/ai_hola_cb.c b/digital/io/src/ai_hola_cb.c new file mode 100644 index 00000000..6b76b058 --- /dev/null +++ b/digital/io/src/ai_hola_cb.c @@ -0,0 +1,165 @@ +/* ai_hola_cb.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "fsm.h" +#include "ai_cb.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; + +/* + * HOLA_IDLE =start=> + * => HOLA_WAIT_HOLA + */ +fsm_branch_t +ai__HOLA_IDLE__start (void) +{ + return ai_next (HOLA_IDLE, start); +} + +/* + * HOLA_WAIT_HOLA =hola_start=> + * => HOLA_WAIT_JACK_IN + */ +fsm_branch_t +ai__HOLA_WAIT_HOLA__hola_start (void) +{ + return ai_next (HOLA_WAIT_HOLA, hola_start); +} + +/* + * HOLA_WAIT_JACK_IN =jack_inserted_into_bot=> + * => HOLA_WAIT_JACK_OUT + */ +fsm_branch_t +ai__HOLA_WAIT_JACK_IN__jack_inserted_into_bot (void) +{ + return ai_next (HOLA_WAIT_JACK_IN, jack_inserted_into_bot); +} + +/* + * HOLA_WAIT_JACK_OUT =jack_removed_from_bot=> + * => HOLA_ROUNDS + */ +fsm_branch_t +ai__HOLA_WAIT_JACK_OUT__jack_removed_from_bot (void) +{ + return ai_next (HOLA_WAIT_JACK_OUT, jack_removed_from_bot); +} + +/* + * HOLA_ROUNDS =asserv_last_cmd_ack=> + * no_timeout => HOLA_ROUNDS + * move clamp rounds + * timeout => HOLA_UP + * move up + * rotate + */ +fsm_branch_t +ai__HOLA_ROUNDS__asserv_last_cmd_ack (void) +{ + 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 ai_next_branch (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 ai_next_branch (HOLA_ROUNDS, asserv_last_cmd_ack, timeout); + } +} + +/* + * HOLA_UP =bot_move_succeed=> + * => HOLA_CENTER + * rotate to center + */ +fsm_branch_t +ai__HOLA_UP__bot_move_succeed (void) +{ + 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 ai_next (HOLA_UP, bot_move_succeed); +} + +/* + * HOLA_CENTER =bot_move_succeed=> + * => HOLA_CLAP_CLOSE + * close clamp + */ +fsm_branch_t +ai__HOLA_CENTER__bot_move_succeed (void) +{ + mimot_motor0_clamp (BOT_CLAMP_ZERO_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_ZERO_SPEED, 0); + return ai_next (HOLA_CENTER, bot_move_succeed); +} + +/* + * HOLA_CLAP_CLOSE =clamp_succeed=> + * => HOLA_CLAP_OPEN + * open clamp + */ +fsm_branch_t +ai__HOLA_CLAP_CLOSE__clamp_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 (HOLA_CLAP_CLOSE, clamp_succeed); +} + +/* + * HOLA_CLAP_OPEN =clamp_succeed=> + * => HOLA_CLAP_CLOSE + * close clamp + */ +fsm_branch_t +ai__HOLA_CLAP_OPEN__clamp_succeed (void) +{ + mimot_motor0_clamp (BOT_CLAMP_SPEED, 0); + mimot_motor1_clamp (BOT_CLAMP_SPEED, 0); + return ai_next (HOLA_CLAP_OPEN, clamp_succeed); +} + + diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 03f3ea65..e4461948 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -52,6 +52,16 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); } +/* + * LOADER_WAIT_JACK_IN =hola_start=> + * => LOADER_IDLE + */ +fsm_branch_t +ai__LOADER_WAIT_JACK_IN__hola_start (void) +{ + return ai_next (LOADER_WAIT_JACK_IN, hola_start); +} + /* * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=> * => LOADER_INIT_ELEVATOR_ZERO diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index 82a860a8..2d0e1933 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -67,7 +67,7 @@ typedef u8 fsm_branch_t; typedef fsm_branch_t (*fsm_transition_t) (void); /** Maximum number of active states. */ -#define FSM_ACTIVE_STATES_MAX 4 +#define FSM_ACTIVE_STATES_MAX 5 /** Context of an FSM. */ struct fsm_t diff --git a/digital/io/src/fsm_templates/template_hola_cb_skel.c b/digital/io/src/fsm_templates/template_hola_cb_skel.c new file mode 100644 index 00000000..980718da --- /dev/null +++ b/digital/io/src/fsm_templates/template_hola_cb_skel.c @@ -0,0 +1,11 @@ +/* + * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! + * + * Skeleton for %(name)s hola callbacks implementation. + * +%(*comments)s */ +#include "common.h" +#include "fsm.h" +#include "%(prefix)s_cb.h" + +%(states_template,template_cb_impl.c,origin=hola)s diff --git a/digital/io/src/hola.c b/digital/io/src/hola.c new file mode 100644 index 00000000..1fd15f18 --- /dev/null +++ b/digital/io/src/hola.c @@ -0,0 +1,25 @@ +/* hola.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + diff --git a/digital/io/src/hola.fsm b/digital/io/src/hola.fsm new file mode 100644 index 00000000..ca219491 --- /dev/null +++ b/digital/io/src/hola.fsm @@ -0,0 +1,59 @@ +# Hola FSM +hola + entertain people + +States: + *HOLA_IDLE + waiting start + HOLA_WAIT_HOLA + wait hola signal + HOLA_WAIT_JACK_IN + wait until the jack is inserted for the first time + HOLA_WAIT_JACK_OUT + wait until the jack is removed to initialise robot + HOLA_ROUNDS + moving clamps in rounds + HOLA_UP + moving clamp up + HOLA_CENTER + HOLA_CLAP_CLOSE + HOLA_CLAP_OPEN + +Events: + hola_start + start hola + +HOLA_IDLE: + start -> HOLA_WAIT_HOLA + +HOLA_WAIT_HOLA: + hola_start -> HOLA_WAIT_JACK_IN + +HOLA_WAIT_JACK_IN: + jack_inserted_into_bot -> HOLA_WAIT_JACK_OUT + +HOLA_WAIT_JACK_OUT: + jack_removed_from_bot -> HOLA_ROUNDS + +HOLA_ROUNDS: + asserv_last_cmd_ack: no_timeout -> . + move clamp rounds + asserv_last_cmd_ack: timeout -> HOLA_UP + move up + rotate + +HOLA_UP: + bot_move_succeed -> HOLA_CENTER + rotate to center + +HOLA_CENTER: + bot_move_succeed -> HOLA_CLAP_CLOSE + close clamp + +HOLA_CLAP_CLOSE: + clamp_succeed -> HOLA_CLAP_OPEN + open clamp + +HOLA_CLAP_OPEN: + clamp_succeed -> HOLA_CLAP_CLOSE + close clamp diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 83972230..f2ee3e44 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -59,8 +59,6 @@ Events: asserv failure result elevator_unload_position elevator above unloading position - clamp_succeed - asserv success result clamp_failed asserv failure result gate_succeed @@ -87,6 +85,7 @@ LOADER_IDLE: LOADER_WAIT_JACK_IN: jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT + hola_start -> LOADER_IDLE LOADER_WAIT_JACK_OUT: jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 63de17b1..43ba07ad 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -198,7 +198,12 @@ main_event_to_fsm (void) if (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element); if (!IO_GET (CONTACT_STRATEGY)) - loader_down (); + { + if (switch_get_color ()) + loader_down (); + else + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_hola_start); + } /* Jack */ if (switch_get_jack ()) diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index f9a95994..faa049f5 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -51,8 +51,11 @@ Events: timeout in cycle count. asserv_last_cmd_ack last command sent to the asserv board has been acquitted. + clamp_succeed + asserv success result import init.fsm +import hola.fsm import loader.fsm import move.fsm -- cgit v1.2.3