From 276f7610f1a44b065407fa276dfeee970034aa7d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 2 May 2010 10:08:55 +0200 Subject: digital/io/src: add loader FSM --- digital/io/src/Makefile | 2 +- digital/io/src/ai.conf | 1 + digital/io/src/ai_loader_cb.c | 99 ++++++++++++++++++++++ digital/io/src/fsm.h | 2 +- .../io/src/fsm_templates/template_loader_cb_skel.c | 11 +++ digital/io/src/loader.c | 27 ++++++ digital/io/src/loader.fsm | 41 +++++++++ digital/io/src/loader.h | 28 ++++++ digital/io/src/main.c | 5 ++ digital/io/src/top.fsm | 1 + 10 files changed, 215 insertions(+), 2 deletions(-) create mode 100644 digital/io/src/ai_loader_cb.c create mode 100644 digital/io/src/fsm_templates/template_loader_cb_skel.c create mode 100644 digital/io/src/loader.c create mode 100644 digital/io/src/loader.fsm create mode 100644 digital/io/src/loader.h (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index c0299735..38c43a5e 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 +FSM_SOURCES := top init move loader # Include FSM makefile. include $(BASE)/make/Makefile.fsm diff --git a/digital/io/src/ai.conf b/digital/io/src/ai.conf index 53429b68..b6527759 100644 --- a/digital/io/src/ai.conf +++ b/digital/io/src/ai.conf @@ -8,3 +8,4 @@ template_cb.h = %_cb.h 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 diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c new file mode 100644 index 00000000..18cdc4cb --- /dev/null +++ b/digital/io/src/ai_loader_cb.c @@ -0,0 +1,99 @@ +/* ai_loader_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 "bot.h" + +/* + * LOADER_IDLE =start=> + * => LOADER_WAIT_JACK_IN + */ +fsm_branch_t +ai__LOADER_IDLE__start (void) +{ + return ai_next (LOADER_IDLE, start); +} + +/* + * LOADER_WAIT_JACK_IN =jack_inserted_into_bot=> + * => LOADER_WAIT_JACK_OUT + */ +fsm_branch_t +ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void) +{ + return ai_next (LOADER_WAIT_JACK_IN, jack_inserted_into_bot); +} + +/* + * LOADER_WAIT_JACK_OUT =jack_removed_from_bot=> + * => LOADER_INIT_ELEVATOR_ZERO + * find elevator zero + */ +fsm_branch_t +ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void) +{ + asserv_motor0_zero_position (-BOT_ELEVATOR_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 + */ +fsm_branch_t +ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, + BOT_ELEVATOR_SPEED); + return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed); +} + +/* + * LOADER_INIT_ELEVATOR_UP =elevator_succeed=> + * => LOADER_IDLE + */ +fsm_branch_t +ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void) +{ + return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_succeed); +} + +/* + * LOADER_INIT_ELEVATOR_UP =elevator_failed=> + * => LOADER_IDLE + * initialisation failure + */ +fsm_branch_t +ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void) +{ + /* Move so that the operator notice it. */ + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP / 3, + BOT_ELEVATOR_ZERO_SPEED); + return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed); +} + diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index 72dfc56e..f06c92fb 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -56,7 +56,7 @@ typedef u8 fsm_branch_t; typedef fsm_branch_t (*fsm_transition_t) (void); /** Maximum number of active states. */ -#define FSM_ACTIVE_STATES_MAX 3 +#define FSM_ACTIVE_STATES_MAX 4 /** Context of an FSM. */ struct fsm_t diff --git a/digital/io/src/fsm_templates/template_loader_cb_skel.c b/digital/io/src/fsm_templates/template_loader_cb_skel.c new file mode 100644 index 00000000..06398952 --- /dev/null +++ b/digital/io/src/fsm_templates/template_loader_cb_skel.c @@ -0,0 +1,11 @@ +/* + * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! + * + * Skeleton for %(name)s loader callbacks implementation. + * +%(*comments)s */ +#include "common.h" +#include "fsm.h" +#include "%(prefix)s_cb.h" + +%(states_template,template_cb_impl.c,origin=loader)s diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c new file mode 100644 index 00000000..bf297161 --- /dev/null +++ b/digital/io/src/loader.c @@ -0,0 +1,27 @@ +/* loader.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 "loader.h" + diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm new file mode 100644 index 00000000..c89e8931 --- /dev/null +++ b/digital/io/src/loader.fsm @@ -0,0 +1,41 @@ +# loader FSM +loader + Handle Eurobot 2010 robot clamp, elevator and output door. + +States: + *LOADER_IDLE + wait start + LOADER_WAIT_JACK_IN + wait until the jack is inserted for the first time + LOADER_WAIT_JACK_OUT + wait until the jack is removed to initialise robot + LOADER_INIT_ELEVATOR_ZERO + move the elevator down to find zero position + LOADER_INIT_ELEVATOR_UP + move the elevator up so that the robot can go to the wall + +Events: + elevator_succeed + asserv success result + elevator_failed + asserv failure result + +LOADER_IDLE: + start -> LOADER_WAIT_JACK_IN + +LOADER_WAIT_JACK_IN: + jack_inserted_into_bot -> LOADER_WAIT_JACK_OUT + +LOADER_WAIT_JACK_OUT: + jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO + find elevator zero + +LOADER_INIT_ELEVATOR_ZERO: + elevator_succeed -> LOADER_INIT_ELEVATOR_UP + move elevator up + +LOADER_INIT_ELEVATOR_UP: + elevator_succeed -> LOADER_IDLE + elevator_failed -> LOADER_IDLE + initialisation failure + diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h new file mode 100644 index 00000000..897763ab --- /dev/null +++ b/digital/io/src/loader.h @@ -0,0 +1,28 @@ +#ifndef loader_h +#define loader_h +/* loader.h */ +/* 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. + * + * }}} */ + +#endif /* loader_h */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index bf7d06ae..b0973d66 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -139,6 +139,11 @@ main_event_to_fsm (void) else if (move_status == failure) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_bot_move_failed); + if (motor0_status == success) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_succeed); + else if (motor0_status == failure) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed); + /* Jack */ if (switch_get_jack ()) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot); diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 6340984e..5a5df1ad 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -32,6 +32,7 @@ Events: import init.fsm import move.fsm +import loader.fsm IDLE: start -> WAIT_INIT_TO_FINISH -- cgit v1.2.3