summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-03-23 08:21:17 +0100
committerNicolas Schodet2013-03-23 13:03:02 +0100
commita4ba86186d0888953a1a195f8c38571d40b7b6d1 (patch)
tree4151ee280a4632626c92f3a0900c81a900f66485 /digital
parent4e7a9fa756eb4093812074125a17e6825b2744c6 (diff)
digital/io-hub/src/apbirthday: add initialisation FSM and deps
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/tools/apbirthday.py5
-rw-r--r--digital/io-hub/src/apbirthday/Makefile2
-rw-r--r--digital/io-hub/src/apbirthday/bot.hh59
-rw-r--r--digital/io-hub/src/apbirthday/init_defs.hh47
-rw-r--r--digital/io-hub/src/apbirthday/move.cc32
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc24
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh3
-rw-r--r--digital/io-hub/src/common-cc/init.cc164
8 files changed, 328 insertions, 8 deletions
diff --git a/digital/ai/tools/apbirthday.py b/digital/ai/tools/apbirthday.py
index 1dad3f0f..44f1184c 100644
--- a/digital/ai/tools/apbirthday.py
+++ b/digital/ai/tools/apbirthday.py
@@ -17,10 +17,9 @@ class Robot:
import simu.robots.apbirthday.model.bag as robot_model
import simu.robots.apbirthday.view.bag as robot_view
- # TODO: use right position.
robot_start_pos = {
- False: (250, 2000 - 250, math.radians (0)),
- True: (3000 - 250, 2000 - 250, math.radians (180))
+ False: (650, 2000 - 250, math.radians (90)),
+ True: (3000 - 650, 2000 - 250, math.radians (90))
}
client_nb = 3
diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile
index 362e7476..d0567efd 100644
--- a/digital/io-hub/src/apbirthday/Makefile
+++ b/digital/io-hub/src/apbirthday/Makefile
@@ -4,7 +4,7 @@ TARGETS = host stm32f4
PROGS = apbirthday
apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \
i2c_queue.cc asserv.cc \
- top.cc \
+ top.cc init.cc move.cc \
angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \
$(AVR_SOURCES)
diff --git a/digital/io-hub/src/apbirthday/bot.hh b/digital/io-hub/src/apbirthday/bot.hh
new file mode 100644
index 00000000..5817a478
--- /dev/null
+++ b/digital/io-hub/src/apbirthday/bot.hh
@@ -0,0 +1,59 @@
+#ifndef bot_hh
+#define bot_hh
+// io-hub - Modular Input/Output. {{{
+//
+// Copyright (C) 2013 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.
+//
+// }}}
+
+/// Robot specific definitions.
+
+/// Scaling factor, millimeter per step.
+#ifdef HOST
+# define BOT_SCALE 0.0395840674352314
+#else
+# define BOT_SCALE 0.0317975134344
+#endif
+
+/// Distance from the robot axis to the front.
+#define BOT_SIZE_FRONT 102
+/// Distance from the robot axis to the back.
+#define BOT_SIZE_BACK 108
+/// Distance from the robot axis to the side.
+#define BOT_SIZE_SIDE 140
+/// Maximum distance from the robot base center to one of its edge.
+#define BOT_SIZE_RADIUS 177
+
+/// Distance between the front contact point and the robot center.
+#define BOT_FRONT_CONTACT_DIST BOT_SIZE_FRONT
+/// Angle error at the front contact point.
+#define BOT_BACK_CONTACT_ANGLE_ERROR_DEG 0
+
+/// Speed used for initialisation.
+#ifdef HOST
+# define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20
+#else
+# define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10
+#endif
+/// Normal cruise speed.
+#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
+
+#endif // bot_hh
diff --git a/digital/io-hub/src/apbirthday/init_defs.hh b/digital/io-hub/src/apbirthday/init_defs.hh
new file mode 100644
index 00000000..ae2099ca
--- /dev/null
+++ b/digital/io-hub/src/apbirthday/init_defs.hh
@@ -0,0 +1,47 @@
+#ifndef init_defs_hh
+#define init_defs_hh
+// io-hub - Modular Input/Output. {{{
+//
+// Copyright (C) 2013 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.
+//
+// }}}
+
+/// Parameters to push the first wall.
+#define INIT_FIRST_WALL_PUSH \
+ Asserv::FORWARD, pg_x (650), pg_y (pg_length - BOT_FRONT_CONTACT_DIST), \
+ pg_a_deg (90 + BOT_BACK_CONTACT_ANGLE_ERROR_DEG)
+/// Parameters to go away from the first wall.
+#define INIT_FIRST_WALL_AWAY (-(400 - BOT_FRONT_CONTACT_DIST))
+/// Parameter to face the second wall.
+#define INIT_SECOND_WALL_ANGLE pg_a_deg (180)
+/// Parameters to push the second wall.
+#define INIT_SECOND_WALL_PUSH \
+ Asserv::FORWARD, pg_x (BOT_FRONT_CONTACT_DIST), -1, -1
+/// Parameters to go away from the second wall.
+#define INIT_SECOND_WALL_AWAY (-(200 - BOT_FRONT_CONTACT_DIST))
+/// Parameter to face the start position.
+#define INIT_START_POSITION_ANGLE \
+ pg_a_deg (-90)
+/// Start position.
+#define INIT_START_POSITION \
+ pg_position_deg (200, pg_length - 600, 0), Asserv::REVERT_OK
+
+#endif // init_defs_hh
diff --git a/digital/io-hub/src/apbirthday/move.cc b/digital/io-hub/src/apbirthday/move.cc
new file mode 100644
index 00000000..6a1041d0
--- /dev/null
+++ b/digital/io-hub/src/apbirthday/move.cc
@@ -0,0 +1,32 @@
+// io-hub - Modular Input/Output. {{{
+//
+// Copyright (C) 2013 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 "move.hh"
+
+#include "robot.hh"
+
+FSM_EVENTS (
+ // Report from asserv after a successful move command.
+ robot_move_success,
+ // Report from asserv after a failed move command.
+ robot_move_failure)
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index 83a9928d..cfdc0c19 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -23,6 +23,8 @@
// }}}
#include "robot.hh"
+#include "bot.hh"
+
#include "ucoolib/arch/arch.hh"
#include "ucoolib/utils/bytes.hh"
@@ -30,7 +32,7 @@ Robot *robot;
Robot::Robot ()
: main_i2c_queue_ (hardware.main_i2c),
- asserv (main_i2c_queue_, scale),
+ asserv (main_i2c_queue_, BOT_SCALE),
dev_proto (*this, hardware.dev_uart),
zb_proto (*this, hardware.zb_uart),
usb_proto (*this, hardware.usb)
@@ -60,6 +62,26 @@ Robot::main_loop ()
bool
Robot::fsm_gen_event ()
{
+ // If an event is handled, stop generating any other event, because a
+ // transition may have invalidated the current robot state.
+#define fsm_handle_and_return(event) \
+ do { if (ANGFSM_HANDLE (AI, event)) return true; } while (0)
+ // FSM timeouts.
+ if (FSM_HANDLE_TIMEOUT (AI))
+ return true;
+ // Motor status.
+ Motor::Status robot_move_status;
+ robot_move_status = asserv.get_status ();
+ if (robot_move_status == Motor::SUCCESS)
+ fsm_handle_and_return (robot_move_success);
+ else if (robot_move_status == Motor::FAILURE)
+ fsm_handle_and_return (robot_move_failure);
+ // Jack. TODO: bounce filter.
+ if (!hardware.raw_jack.get ())
+ fsm_handle_and_return (jack_inserted);
+ else
+ fsm_handle_and_return (jack_removed);
+ // FSM queue.
if (fsm_queue.poll ())
{
FsmQueue::Event event = fsm_queue.pop ();
diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh
index d9590e59..1c6af976 100644
--- a/digital/io-hub/src/apbirthday/robot.hh
+++ b/digital/io-hub/src/apbirthday/robot.hh
@@ -33,9 +33,6 @@
class Robot : public ucoo::Proto::Handler
{
public:
- /// Scaling factor, millimeter per step.
- static const float scale = 0.0395840674352314;
- public:
/// Initialise robot singleton.
Robot ();
/// Main program loop.
diff --git a/digital/io-hub/src/common-cc/init.cc b/digital/io-hub/src/common-cc/init.cc
new file mode 100644
index 00000000..6d8b9255
--- /dev/null
+++ b/digital/io-hub/src/common-cc/init.cc
@@ -0,0 +1,164 @@
+// io-hub - Modular Input/Output. {{{
+//
+// Copyright (C) 2013 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 "init_defs.hh"
+#include "robot.hh"
+#include "bot.hh"
+#include "playground.hh"
+
+// 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 (
+ // Jack is inserted.
+ jack_inserted,
+ // Jack is removed.
+ jack_removed,
+ // Sent to initialise actuators.
+ init_actuators,
+ // Sent when init is done.
+ init_done,
+ // Sent to start round.
+ init_start_round)
+
+FSM_START_WITH (INIT_START)
+
+FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT)
+{
+}
+
+FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed,
+ INIT_INITIALISING_ACTUATORS)
+{
+ robot->fsm_queue.post (FSM_EVENT (init_actuators));
+}
+
+FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT)
+{
+ team_color = robot->hardware.ihm_color.get ()
+ ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT;
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 250, INIT_FINDING_FIRST_WALL)
+{
+ robot->asserv.set_speed (BOT_SPEED_INIT);
+ robot->asserv.push_wall (INIT_FIRST_WALL_PUSH);
+}
+
+FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success,
+ INIT_GOING_AWAY_FIRST_WALL)
+{
+ robot->asserv.move_distance (INIT_FIRST_WALL_AWAY);
+}
+
+FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success,
+ INIT_FACING_SECOND_WALL)
+{
+ robot->asserv.goto_angle (INIT_SECOND_WALL_ANGLE);
+}
+
+FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success,
+ INIT_WAITING_AFTER_FACING_SECOND_WALL)
+{
+}
+
+FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 250 / 2,
+ INIT_FINDING_SECOND_WALL)
+{
+ robot->asserv.push_wall (INIT_SECOND_WALL_PUSH);
+}
+
+FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success,
+ INIT_GOING_AWAY_SECOND_WALL)
+{
+ robot->asserv.move_distance (INIT_SECOND_WALL_AWAY);
+}
+
+#ifdef INIT_START_POSITION_ANGLE
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_FACING_START_POSITION)
+{
+ robot->asserv.goto_angle (INIT_START_POSITION_ANGLE);
+}
+
+FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ robot->asserv.goto_xya (INIT_START_POSITION);
+}
+
+#else
+
+FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success,
+ INIT_GOING_TO_START_POSITION)
+{
+ robot->asserv.goto_xya (INIT_START_POSITION);
+}
+
+#endif
+
+FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success,
+ INIT_WAITING_SECOND_JACK_OUT)
+{
+ robot->fsm_queue.post (FSM_EVENT (init_done));
+ robot->asserv.set_speed (BOT_SPEED_NORMAL);
+}
+
+FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED)
+{
+ // TODO: robot->chrono.start ();
+ robot->fsm_queue.post (FSM_EVENT (init_start_round));
+}
+