summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/ai.conf1
-rw-r--r--digital/io/src/ai_hola_cb.c165
-rw-r--r--digital/io/src/ai_loader_cb.c10
-rw-r--r--digital/io/src/fsm.h2
-rw-r--r--digital/io/src/fsm_templates/template_hola_cb_skel.c11
-rw-r--r--digital/io/src/hola.c25
-rw-r--r--digital/io/src/hola.fsm59
-rw-r--r--digital/io/src/loader.fsm3
-rw-r--r--digital/io/src/main.c7
-rw-r--r--digital/io/src/top.fsm3
11 files changed, 283 insertions, 5 deletions
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
@@ -53,6 +53,16 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
}
/*
+ * 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
* find 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