From 94ad65245035203845f264f89a05d564b680869d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 3 Apr 2013 08:16:40 +0200 Subject: digital/io-hub/src/apbirthday: add move FSM --- digital/io-hub/src/apbirthday/move.cc | 32 -------------------------------- digital/io-hub/src/apbirthday/robot.cc | 8 +++++--- digital/io-hub/src/apbirthday/robot.hh | 6 ++++++ 3 files changed, 11 insertions(+), 35 deletions(-) delete mode 100644 digital/io-hub/src/apbirthday/move.cc (limited to 'digital/io-hub/src/apbirthday') diff --git a/digital/io-hub/src/apbirthday/move.cc b/digital/io-hub/src/apbirthday/move.cc deleted file mode 100644 index 6a1041d0..00000000 --- a/digital/io-hub/src/apbirthday/move.cc +++ /dev/null @@ -1,32 +0,0 @@ -// 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 6352cca9..f76903f9 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -167,6 +167,9 @@ Robot::fsm_gen_event () if (ANGFSM_HANDLE_VAR (AI, event)) return true; } + // Check obstacles. + if (move.check_obstacles ()) + return true; return false; } @@ -193,9 +196,8 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size (int16_t) ucoo::bytes_pack (args[0], args[1]), (int16_t) ucoo::bytes_pack (args[2], args[3]), }; - asserv.stop (); - // TODO: use move FSM. - asserv.goto_xy (pos, Asserv::DirectionConsign (args[4])); + move.stop (); + move.start (pos, Asserv::DirectionConsign (args[4])); } break; case c ('f', 2): diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index e9275388..fa2a2185 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -31,6 +31,8 @@ #include "outputs.hh" #include "radar_2013.hh" #include "obstacles.hh" +#include "move.hh" +#include "path.hh" #include "candles.hh" #include "ucoolib/base/proto/proto.hh" @@ -81,6 +83,10 @@ class Robot : public ucoo::Proto::Handler public: /// Obstacle database. Obstacles obstacles; + /// Path finding. + Path path; + /// Move FSM. + Move move; /// Candles. Candles candles; private: -- cgit v1.2.3