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/common-cc/move.cc | 386 +++++++++++++++++++++++++++++++++++ 1 file changed, 386 insertions(+) create mode 100644 digital/io-hub/src/common-cc/move.cc (limited to 'digital/io-hub/src/common-cc/move.cc') 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 (); +} + -- cgit v1.2.3