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/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 -- 6 files changed, 162 insertions(+), 5 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 (limited to 'digital/io-hub/src/apbirthday') 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 (); -- cgit v1.2.3