summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/common-cc/move.cc
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/common-cc/move.cc')
-rw-r--r--digital/io-hub/src/common-cc/move.cc386
1 files changed, 386 insertions, 0 deletions
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 <cstdlib>
+#include <cmath>
+
+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 ();
+}
+