summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-20 08:27:02 +0200
committerNicolas Schodet2011-05-20 08:27:02 +0200
commit2277c9a7aa41c347ba04fca8e202c77b43618802 (patch)
treea73904b3ed877677411caecadaf83ee5b3ff6319 /digital
parentc3274377f05073c6d40265ab7849df2a72cfd54e (diff)
digital/{ai,io-hub}: add init FSM
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/src/fsm/init.c188
-rw-r--r--digital/ai/tools/robospierre.py5
-rw-r--r--digital/io-hub/src/robospierre/Makefile2
-rw-r--r--digital/io-hub/src/robospierre/bot.h10
-rw-r--r--digital/io-hub/src/robospierre/clamp.c6
-rw-r--r--digital/io-hub/src/robospierre/init_defs.h50
-rw-r--r--digital/io-hub/src/robospierre/main.c14
7 files changed, 265 insertions, 10 deletions
diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c
new file mode 100644
index 00000000..1f933ce1
--- /dev/null
+++ b/digital/ai/src/fsm/init.c
@@ -0,0 +1,188 @@
+/* init.c */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 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 "asserv.h"
+#include "contact.h"
+#include "chrono.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
+
+#include "bot.h"
+#include "init_defs.h"
+
+/*
+ * Initialise robot position with a calibration procedure.
+ */
+
+FSM_STATES (
+ /* Initial state, waiting for jack. */
+ INIT_START,
+ /* After first jack insertion, waiting for removal to initialise actuators. */
+ INIT_WAITING_FIRST_JACK_OUT,
+ /* Initialising actuators, then waiting for second jack insertion. */
+ INIT_INITIALISING_ACTUATORS,
+ /* After second jack insertion, waiting the operator remove its
+ * hand. */
+ INIT_WAITING_HANDS_OUT,
+ /* Finding the first wall. */
+ INIT_FINDING_FIRST_WALL,
+ /* Going away from the wall. */
+ INIT_GOING_AWAY_FIRST_WALL,
+ /* Turning to face the other wall. */
+ INIT_FACING_SECOND_WALL,
+ /* Waiting after rotation for robot to stabilize. */
+ INIT_WAITING_AFTER_FACING_SECOND_WALL,
+ /* Finding the second wall. */
+ INIT_FINDING_SECOND_WALL,
+ /* Going away from the wall. */
+ INIT_GOING_AWAY_SECOND_WALL,
+#ifdef INIT_START_POSITION_ANGLE
+ /* Facing the start position. */
+ INIT_FACING_START_POSITION,
+#endif
+ /* Going to start position. */
+ INIT_GOING_TO_START_POSITION,
+ /* Waiting for the round start (waiting for the jack). */
+ INIT_WAITING_SECOND_JACK_OUT,
+ /* Initialisation finished, nothing else to do. */
+ INIT_FINISHED)
+
+FSM_EVENTS (
+ /* XXX: temporarily here. */
+ robot_move_success,
+ robot_move_failure,
+ /* Jack is inserted. */
+ jack_inserted,
+ /* Jack is removed. */
+ jack_removed,
+ /* Sent to initialise actuators. */
+ init_actuators,
+ /* Sent to start round. */
+ init_start_round)
+
+FSM_START_WITH (INIT_START)
+
+FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT)
+{
+ return FSM_NEXT (INIT_START, jack_inserted);
+}
+
+FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed,
+ INIT_INITIALISING_ACTUATORS)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, init_actuators));
+ return FSM_NEXT (INIT_WAITING_FIRST_JACK_OUT, jack_removed);
+}
+
+FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT)
+{
+ team_color = contact_get_color ();
+ return FSM_NEXT (INIT_INITIALISING_ACTUATORS, jack_inserted);
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 225, INIT_FINDING_FIRST_WALL)
+{
+ asserv_set_speed (BOT_SPEED_INIT);
+ asserv_push_the_wall (INIT_FIRST_WALL_PUSH);
+ return FSM_NEXT_TIMEOUT (INIT_WAITING_HANDS_OUT);
+}
+
+FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success,
+ INIT_GOING_AWAY_FIRST_WALL)
+{
+ asserv_move_linearly (INIT_FIRST_WALL_AWAY);
+ return FSM_NEXT (INIT_FINDING_FIRST_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success,
+ INIT_FACING_SECOND_WALL)
+{
+ asserv_goto_angle (INIT_SECOND_WALL_ANGLE);
+ return FSM_NEXT (INIT_GOING_AWAY_FIRST_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success,
+ INIT_WAITING_AFTER_FACING_SECOND_WALL)
+{
+ return FSM_NEXT (INIT_FACING_SECOND_WALL, robot_move_success);
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 225 / 2,
+ INIT_FINDING_SECOND_WALL)
+{
+ asserv_push_the_wall (INIT_SECOND_WALL_PUSH);
+ return FSM_NEXT_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL);
+}
+
+FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success,
+ INIT_GOING_AWAY_SECOND_WALL)
+{
+ asserv_move_linearly (INIT_SECOND_WALL_AWAY);
+ return FSM_NEXT (INIT_FINDING_SECOND_WALL, robot_move_success);
+}
+
+#ifdef INIT_START_POSITION_ANGLE
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_FACING_START_POSITION)
+{
+ asserv_goto_angle (INIT_START_POSITION_ANGLE);
+ return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success);
+}
+
+FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ asserv_goto_xya (INIT_START_POSITION);
+ return FSM_NEXT (INIT_FACING_START_POSITION, robot_move_success);
+}
+
+#else
+
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ asserv_goto_xya (INIT_START_POSITION);
+ return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success);
+}
+
+#endif
+
+FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success,
+ INIT_WAITING_SECOND_JACK_OUT)
+{
+ asserv_set_speed (BOT_SPEED_NORMAL);
+ return FSM_NEXT (INIT_GOING_TO_START_POSITION, robot_move_success);
+}
+
+FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED)
+{
+ chrono_init ();
+ fsm_queue_post_event (FSM_EVENT (AI, init_start_round));
+ return FSM_NEXT (INIT_WAITING_SECOND_JACK_OUT, jack_removed);
+}
+
diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py
index 8d5db276..e8384f6c 100644
--- a/digital/ai/tools/robospierre.py
+++ b/digital/ai/tools/robospierre.py
@@ -34,7 +34,8 @@ class Robot:
self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time,
**io_hub.init.host['robospierre'])
self.robot_start_pos = {
- False: (700, 2100 - 250, math.radians (-270)),
- True: (3000 - 700, 2100 - 250, math.radians (-270))
+ # In real life, better place the robot in green zone.
+ False: (300, 2100 - 200, math.radians (180)),
+ True: (3000 - 300, 2100 - 200, math.radians (0))
}
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index 20e9dee0..d69918f5 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -5,7 +5,7 @@ PROGS = io_hub
# Sources to compile.
io_hub_SOURCES = main.c \
clamp.c logistic.c \
- fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
+ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
pwm.avr.c pwm.host.c \
contact.avr.c contact.host.c \
twi_master.c asserv.c mimot.c \
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index e014321e..a986c015 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -34,6 +34,16 @@
# define BOT_SCALE 0.0415178942124
#endif
+/** Distance between the front contact point and the robot center. */
+#define BOT_FRONT_CONTACT_DIST_MM 150
+/** Angle error at the front contact point. */
+#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0
+
+/** Speed used for initialisation. */
+#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10
+/** Normal cruise speed. */
+#define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20
+
#ifdef HOST
# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 6c063979..583cadb5 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -83,8 +83,6 @@ FSM_STATES (
CLAMP_MOVE_DST_CLAMP_OPENING)
FSM_EVENTS (
- /* Here for the moment, to be moved later. */
- start,
/* New element inside bottom slot. */
clamp_new_element,
/* Order to drop elements. */
@@ -325,10 +323,10 @@ clamp_route (void)
/* CLAMP FSM */
-FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE)
+FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE)
{
clamp_move (CLAMP_SLOT_FRONT_MIDDLE);
- return FSM_NEXT (CLAMP_START, start);
+ return FSM_NEXT (CLAMP_START, init_actuators);
}
FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
diff --git a/digital/io-hub/src/robospierre/init_defs.h b/digital/io-hub/src/robospierre/init_defs.h
new file mode 100644
index 00000000..6ce6eaae
--- /dev/null
+++ b/digital/io-hub/src/robospierre/init_defs.h
@@ -0,0 +1,50 @@
+#ifndef init_defs_h
+#define init_defs_h
+/* init_defs.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 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 "playground.h"
+#include "bot.h"
+
+/** Parameters to push the first wall. */
+#define INIT_FIRST_WALL_PUSH \
+ 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \
+ PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG)
+/** Parameters to go away from the first wall. */
+#define INIT_FIRST_WALL_AWAY -500
+/** Parameter to face the second wall. */
+#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90)
+/** Parameters to push the second wall. */
+#define INIT_SECOND_WALL_PUSH \
+ 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1
+/** Parameters to go away from the second wall. */
+#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM)
+/** Parameter to face the start position. */
+#define INIT_START_POSITION_ANGLE PG_A_DEG (0)
+/** Start position. */
+#define INIT_START_POSITION \
+ PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD
+
+#endif /* init_defs_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 97d5bd40..f1b4d8de 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -110,9 +110,15 @@ main_event_to_fsm (void)
/* Update FSM timeouts. */
FSM_HANDLE_TIMEOUT_E (AI);
/* Motor status. */
- asserv_status_e mimot_motor0_status, mimot_motor1_status;
+ asserv_status_e robot_move_status, mimot_motor0_status,
+ mimot_motor1_status;
+ robot_move_status = asserv_move_cmd_status ();
mimot_motor0_status = mimot_motor0_cmd_status ();
mimot_motor1_status = mimot_motor1_cmd_status ();
+ if (robot_move_status == success)
+ FSM_HANDLE_E (AI, robot_move_success);
+ else if (robot_move_status == failure)
+ FSM_HANDLE_E (AI, robot_move_failure);
if (mimot_motor0_status == success
&& mimot_motor1_status == success)
FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
@@ -123,9 +129,11 @@ main_event_to_fsm (void)
/* Clamp specific events. */
if (clamp_handle_event ())
return;
- /* Jack, XXX to be changed! */
+ /* Jack. */
if (!contact_get_jack ())
- FSM_HANDLE_E (AI, start);
+ FSM_HANDLE_E (AI, jack_inserted);
+ else
+ FSM_HANDLE_E (AI, jack_removed);
/* Events from the event queue. */
if (fsm_queue_poll ())
{