From a4ba86186d0888953a1a195f8c38571d40b7b6d1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 23 Mar 2013 08:21:17 +0100 Subject: digital/io-hub/src/apbirthday: add initialisation FSM and deps --- digital/ai/tools/apbirthday.py | 5 +- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/bot.hh | 59 +++++++++++ digital/io-hub/src/apbirthday/init_defs.hh | 47 +++++++++ digital/io-hub/src/apbirthday/move.cc | 32 ++++++ digital/io-hub/src/apbirthday/robot.cc | 24 ++++- digital/io-hub/src/apbirthday/robot.hh | 3 - digital/io-hub/src/common-cc/init.cc | 164 +++++++++++++++++++++++++++++ 8 files changed, 328 insertions(+), 8 deletions(-) create mode 100644 digital/io-hub/src/apbirthday/bot.hh create mode 100644 digital/io-hub/src/apbirthday/init_defs.hh create mode 100644 digital/io-hub/src/apbirthday/move.cc create mode 100644 digital/io-hub/src/common-cc/init.cc (limited to 'digital') 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 @@ -32,9 +32,6 @@ /// Main robot superclass. class Robot : public ucoo::Proto::Handler { - public: - /// Scaling factor, millimeter per step. - static const float scale = 0.0395840674352314; public: /// Initialise robot singleton. Robot (); 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)); +} + -- cgit v1.2.3