summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-02 10:08:55 +0200
committerNicolas Schodet2010-05-02 10:08:55 +0200
commit276f7610f1a44b065407fa276dfeee970034aa7d (patch)
treec1298733f165e6207a2025af9c327e2433f99734
parentd261d97b1497f67503faa04b83a999848381a791 (diff)
digital/io/src: add loader FSM
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/ai.conf1
-rw-r--r--digital/io/src/ai_loader_cb.c99
-rw-r--r--digital/io/src/fsm.h2
-rw-r--r--digital/io/src/fsm_templates/template_loader_cb_skel.c11
-rw-r--r--digital/io/src/loader.c27
-rw-r--r--digital/io/src/loader.fsm41
-rw-r--r--digital/io/src/loader.h28
-rw-r--r--digital/io/src/main.c5
-rw-r--r--digital/io/src/top.fsm1
10 files changed, 215 insertions, 2 deletions
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