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 + digital/io-hub/src/common-cc/move.cc | 386 ++++++++++++++++++++++++++++++ digital/io-hub/src/common-cc/move.hh | 88 +++++++ digital/io-hub/src/common-cc/obstacles.cc | 42 +++- digital/io-hub/src/common-cc/obstacles.hh | 22 ++ digital/io-hub/src/common-cc/path.hh | 42 ++++ 8 files changed, 590 insertions(+), 36 deletions(-) delete mode 100644 digital/io-hub/src/apbirthday/move.cc create mode 100644 digital/io-hub/src/common-cc/move.cc create mode 100644 digital/io-hub/src/common-cc/move.hh create mode 100644 digital/io-hub/src/common-cc/path.hh (limited to 'digital/io-hub/src') 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: diff --git a/digital/io-hub/src/common-cc/move.cc b/digital/io-hub/src/common-cc/move.cc new file mode 100644 index 00000000..27de0c34 --- /dev/null +++ b/digital/io-hub/src/common-cc/move.cc @@ -0,0 +1,386 @@ +// 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" +#include "bot.hh" +#include "playground.hh" + +#include +#include + +void +Move::start (const Position &position, + Asserv::DirectionConsign direction_consign, + int shorten) +{ + // Set parameters. + final_ = position; + with_angle_ = true; + direction_consign_ = direction_consign; + shorten_ = shorten; + // Start the FSM. + robot->fsm_queue.post (FSM_EVENT (move_start)); +} + +void +Move::start (const vect_t &position, + Asserv::DirectionConsign direction_consign, + int shorten) +{ + // Set parameters. + final_.v = position; + with_angle_ = false; + direction_consign_ = direction_consign; + shorten_ = shorten; + // Start the FSM. + robot->fsm_queue.post (FSM_EVENT (move_start)); +} + +void +Move::stop () +{ + // Stop the FSM. + robot->fsm_queue.post (FSM_EVENT (move_stop)); +} + +bool +Move::check_obstacles () +{ + if (FSM_CAN_HANDLE (AI, obstacle_blocking)) + { + Position robot_pos; + robot->asserv.get_position (robot_pos); + if (robot->obstacles.blocking (robot_pos.v, step_)) + if (FSM_HANDLE (AI, obstacle_blocking)) + return true; + } + return false; +} + +FSM_STATES ( + // Waiting for the start order. + MOVE_IDLE, + // Rotating towards next point. + MOVE_ROTATING, + // Moving to a position (intermediate or final). + MOVE_MOVING, + // Brake when a obstacle is seen. + MOVE_BRAKE, + // Moving backward to go away from what is blocking the bot. + MOVE_MOVING_BACK_UP) + +FSM_EVENTS ( + // Report from asserv after a successful move command. + robot_move_success, + // Report from asserv after a failed move command. + robot_move_failure, + // Initialize the FSM and start the movement directly. + move_start, + // Stop movement. + move_stop, + // Movement success. + move_success, + // Movement failure. + move_failure, + // There is an obstacle blocking on path. + obstacle_blocking) + +FSM_START_WITH (MOVE_IDLE) + +void +Move::go () +{ + vect_t dst = step_; + // Modify final point if requested. + if (step_final_move_ && shorten_) + { + // Compute a vector from destination to robot with lenght 'shorten'. + Position robot_position; + robot->asserv.get_position (robot_position); + vect_t v = robot_position.v; + vect_sub (&v, &step_); + int d = vect_norm (&v); + if (d > shorten_) + { + vect_scale_f824 (&v, 0x1000000 / d * shorten_); + vect_translate (&dst, &v); + } + } + if (step_with_angle_) + { + Position dst_pos = { dst, step_angle_ }; + robot->asserv.goto_xya (dst_pos, step_direction_consign_); + } + else + robot->asserv.goto_xy (dst, step_direction_consign_); +} + +Move::NextMove +Move::go_or_rotate (const vect_t &dst, uint16_t angle, bool with_angle, + Asserv::DirectionConsign direction_consign) +{ + Position robot_position; + robot->asserv.get_position (robot_position); + uint16_t robot_angle = robot_position.a; + if (direction_consign & Asserv::BACKWARD) + robot_angle += 0x8000; + // Remember step. + step_ = dst; + step_angle_ = angle; + step_with_angle_ = with_angle; + step_direction_consign_ = direction_consign; + // Compute angle to destination. + vect_t v = dst; vect_sub (&v, &robot_position.v); + uint16_t dst_angle = std::atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); + if (direction_consign & Asserv::BACKWARD) + dst_angle += 0x8000; + int16_t diff_angle = dst_angle - robot_angle; + if ((direction_consign & Asserv::REVERT_OK) + && (diff_angle > 0x4000 || diff_angle < -0x4000)) + dst_angle += 0x8000; + int16_t diff = dst_angle - robot_angle; + // Move or rotate. + if (std::abs (diff) < 0x1000) + { + go (); + return LINEAR; + } + else + { + robot->asserv.goto_angle (dst_angle); + return ANGULAR; + } +} + +Move::NextMove +Move::go_to_next (const vect_t &dst) +{ + NextMove r; + // If it is not the last position. + if (dst.x != final_.v.x || dst.y != final_.v.y) + { + // Not final position. + step_final_move_ = false; + // Goto without angle. + r = go_or_rotate (dst, 0, false, + Asserv::DirectionConsign (direction_consign_ + | (slow_ ? Asserv::REVERT_OK : 0))); + } + else + { + // Final position. + step_final_move_ = true; + // Goto with angle if requested. + r = go_or_rotate (dst, final_.a, with_angle_, direction_consign_); + } + // Next time, do not use slow. + slow_ = false; + return r; +} + +Move::NextMove +Move::path_init () +{ + bool found; + vect_t dst; + // Get the current position + Position current_pos; + robot->asserv.get_position (current_pos); + // Give the current position of the bot to the path module + robot->path.reset (); + //TODO add obstacles. + robot->path.endpoints (current_pos.v, final_.v); + // Update the path module + slow_ = 0; + robot->path.compute (); + found = robot->path.get_next (dst); + // If not found, try to escape. + if (!found) + { + slow_ = 1; + robot->path.compute (8); + found = robot->path.get_next (dst); + } + // If found, go. + if (found) + return go_to_next (dst); + else + return NONE; +} + +Move::NextMove +Move::path_next () +{ + vect_t dst; + robot->path.get_next (dst); + return go_to_next (dst); +} + +FSM_TRANS (MOVE_IDLE, move_start, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + switch (robot->move.path_init ()) + { + default: + case Move::NONE: + robot->fsm_queue.post (FSM_EVENT (move_failure)); + return FSM_BRANCH (no_path_found); + case Move::LINEAR: + return FSM_BRANCH (path_found); + case Move::ANGULAR: + return FSM_BRANCH (path_found_rotate); + } +} + +FSM_TRANS (MOVE_ROTATING, robot_move_success, MOVE_MOVING) +{ + robot->move.go (); +} + +FSM_TRANS (MOVE_ROTATING, robot_move_failure, MOVE_MOVING) +{ + robot->move.go (); +} + +FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, MOVE_MOVING) +{ + robot->move.go (); +} + +FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE) +{ + robot->asserv.stop (); +} + +FSM_TRANS (MOVE_MOVING, robot_move_success, + done, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING) +{ + if (robot->move.step_final_move_) + { + robot->fsm_queue.post (FSM_EVENT (move_success)); + return FSM_BRANCH (done); + } + else + { + if (robot->move.path_next () == Move::LINEAR) + return FSM_BRANCH (path_found); + else + return FSM_BRANCH (path_found_rotate); + } +} + +/// Test if a point is ok to move back. +static bool +move_test_point (const vect_t &p, Direction direction) +{ + int margin = BOT_SIZE_RADIUS; + return p.x >= margin && p.x < pg_width - margin + && p.y >= margin && p.y < pg_length - margin; + // TODO: cake. +} + +static void +move_back_up (void) +{ + int dist, back_dist; + Direction dir = robot->asserv.get_last_moving_direction (); + if (dir == DIRECTION_FORWARD) + { + dist = BOT_SIZE_FRONT + Obstacles::obstacle_radius_mm; + back_dist = -300; + } + else + { + dist = -(BOT_SIZE_BACK + Obstacles::obstacle_radius_mm); + back_dist = 300; + } + // Assume there is an obstacle in front of the robot. + Position robot_pos; + robot->asserv.get_position (robot_pos); + vect_t obstacle_pos; + vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); + vect_translate (&obstacle_pos, &robot_pos.v); + robot->obstacles.add (obstacle_pos); + // Move backward to turn freely. + vect_t back_pos; + vect_from_polar_uf016 (&back_pos, back_dist, robot_pos.a); + vect_translate (&back_pos, &robot_pos.v); + if (!move_test_point (back_pos, dir)) + back_dist /= 8; + robot->asserv.move_distance (back_dist); +} + +FSM_TRANS (MOVE_MOVING, robot_move_failure, MOVE_MOVING_BACK_UP) +{ + move_back_up (); +} + +FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500, MOVE_MOVING_BACK_UP) +{ + move_back_up (); +} + +FSM_TRANS (MOVE_MOVING, obstacle_blocking, MOVE_BRAKE) +{ + robot->asserv.stop (); +} + +FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE) +{ + robot->asserv.stop (); +} + +FSM_TRANS (MOVE_BRAKE, robot_move_success, MOVE_IDLE) +{ + robot->fsm_queue.post (FSM_EVENT (move_failure)); +} + +FSM_TRANS (MOVE_BRAKE, robot_move_failure, MOVE_IDLE) +{ + robot->fsm_queue.post (FSM_EVENT (move_failure)); +} + +FSM_TRANS (MOVE_BRAKE, move_stop, MOVE_IDLE) +{ +} + +FSM_TRANS (MOVE_MOVING_BACK_UP, robot_move_success, MOVE_IDLE) +{ + robot->fsm_queue.post (FSM_EVENT (move_failure)); +} + +FSM_TRANS (MOVE_MOVING_BACK_UP, robot_move_failure, MOVE_IDLE) +{ + robot->fsm_queue.post (FSM_EVENT (move_failure)); +} + +FSM_TRANS (MOVE_MOVING_BACK_UP, move_stop, MOVE_IDLE) +{ + robot->asserv.stop (); +} + diff --git a/digital/io-hub/src/common-cc/move.hh b/digital/io-hub/src/common-cc/move.hh new file mode 100644 index 00000000..c561d763 --- /dev/null +++ b/digital/io-hub/src/common-cc/move.hh @@ -0,0 +1,88 @@ +#ifndef move_hh +#define move_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. +// +// }}} +#include "defs.hh" +#include "asserv.hh" + +/// Move FSM control. +class Move +{ + public: + enum NextMove + { + NONE, + LINEAR, + ANGULAR, + }; + public: + /// Go to a position. + void start (const Position &position, + Asserv::DirectionConsign direction_consign = Asserv::FORWARD, + int shorten = 0); + /// Go to a position with no angle consign. + void start (const vect_t &position, + Asserv::DirectionConsign direction_consign = Asserv::FORWARD, + int shorten = 0); + /// Stop movement. + void stop (); + /// Check for blocking obstacles, return true if an event was handled. + bool check_obstacles (); + /// Go to current step, low level function. + void go (); + /// Go or rotate toward position, return LINEAR or ANGULAR. + NextMove go_or_rotate (const vect_t &dst, uint16_t angle, bool with_angle, + Asserv::DirectionConsign direction_consign); + /// Go to next position computed by path module, to be called by + /// path_init and path_next, return LINEAR or ANGULAR. + NextMove go_to_next (const vect_t &dst); + /// Compute path and go to first position, return NONE, LINEAR or ANGULAR. + NextMove path_init (); + /// Go to next position in path, return LINEAR or ANGULAR. + NextMove path_next (); + public: + /// Final position. + Position final_; + /// Use angle consign for final point. + bool with_angle_; + /// Direction consign of the whole movement. + Asserv::DirectionConsign direction_consign_; + /// Distance to remove from path. + int shorten_; + /// Next step. + vect_t step_; + /// Next step angle. + uint16_t step_angle_; + /// Next step with_angle. + bool step_with_angle_; + /// Next step direction consign. + Asserv::DirectionConsign step_direction_consign_; + /// Next step is the final move. + bool step_final_move_; + /// If true, this means this is a tricky move, slow down, and minimize + /// turns. + bool slow_; +}; + +#endif // move_hh diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc index d98a92ba..1c02cce6 100644 --- a/digital/io-hub/src/common-cc/obstacles.cc +++ b/digital/io-hub/src/common-cc/obstacles.cc @@ -22,6 +22,7 @@ // // }}} #include "obstacles.hh" +#include "bot.hh" extern "C" { #include "modules/math/geometry/distance.h" @@ -99,7 +100,46 @@ Obstacles::add (const vect_t &pos) bool Obstacles::blocking (const vect_t &robot, const vect_t &dest) const { - // TODO + // Stop here if no obstacle. + bool obs_valid = false; + for (int i = 0; !obs_valid && i < obstacles_nb_; i++) + obs_valid = obstacles_[i].valid != 0; + if (!obs_valid) + return false; + // If destination is realy near, stop here. + vect_t vd = dest; vect_sub (&vd, &robot); + int d = vect_norm (&vd); + if (d < epsilon_mm) + return false; + // If destination is near, use clearance to destination point instead of + // stop length. + vect_t t; + if (d < stop_mm) + t = dest; + else + { + vect_scale_f824 (&vd, (1ll << 24) / d * stop_mm); + t = robot; + vect_translate (&t, &vd); + } + // Now, look at obstacles. + for (int i = 0; i < obstacles_nb_; i++) + { + if (!obstacles_[i].valid) + continue; + // Vector from robot to obstacle. + vect_t vo = obstacles_[i].pos; vect_sub (&vo, &robot); + // Ignore if in our back. + int dp = vect_dot_product (&vd, &vo); + if (dp < 0) + continue; + // Check distance. + int od = distance_segment_point (&robot, &t, &obstacles_[i].pos); + if (od > BOT_SIZE_SIDE + clearance_mm / 2 + obstacle_radius_mm) + continue; + // Else, obstacle is blocking. + return true; + } return false; } diff --git a/digital/io-hub/src/common-cc/obstacles.hh b/digital/io-hub/src/common-cc/obstacles.hh index c3d2bc34..486c64b2 100644 --- a/digital/io-hub/src/common-cc/obstacles.hh +++ b/digital/io-hub/src/common-cc/obstacles.hh @@ -44,6 +44,28 @@ class Obstacles /// Return true if there is an obstacle near the robot while going to a /// destination point. bool blocking (const vect_t &robot, const vect_t &dest) const; + public: + /// Estimated obstacle radius. The obstacle may be larger than at the + /// detected edge. + static const int obstacle_radius_mm = 150; + /// Stop distance. Distance under which an obstacle is considered harmful + /// when moving. + static const int stop_mm = 350; + /// Clearance distance. Distance over which an obstacle should be to the + /// side when moving. + /// + /// OK, more explanations: when moving, a rectangle is placed in front of + /// the robot, of length STOP_MM and width 2 * (CLEARANCE_MM + + /// BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is + /// considered in the way. + /// + /// If the destination point is near (< STOP_MM), this reduce the + /// rectangle length. + /// + /// If the destination is really near (< EPSILON_MM), ignore all this. + static const int clearance_mm = 100; + /// Destination distance near enough so that obstacles could be ignored. + static const int epsilon_mm = 70; private: /// Validity period of a new obstacle. static const int valid_new_ = 125; diff --git a/digital/io-hub/src/common-cc/path.hh b/digital/io-hub/src/common-cc/path.hh new file mode 100644 index 00000000..3c3cce9c --- /dev/null +++ b/digital/io-hub/src/common-cc/path.hh @@ -0,0 +1,42 @@ +#ifndef path_hh +#define path_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. +// +// }}} +#include "defs.hh" + +/// Path finding class. +/// TODO: dummy implementation. +class Path +{ + public: + void reset () { } + void obstacle (int index, vect_t c, int r, int factor = 0) { } + void endpoints (const vect_t &src, const vect_t &dst) { dst_ = dst; } + void compute (int factor = 0) { } + bool get_next (vect_t &p) { p = dst_; return true; } + private: + vect_t dst_; +}; + +#endif // path_hh -- cgit v1.2.3