summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-03 08:16:40 +0200
committerNicolas Schodet2013-04-03 21:37:44 +0200
commit94ad65245035203845f264f89a05d564b680869d (patch)
tree85f691d259b20722941cbfb68423d884493797d0 /digital
parent978367ba839a1b2f94a4a5c37c0d57675ffcb1de (diff)
digital/io-hub/src/apbirthday: add move FSM
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc8
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh6
-rw-r--r--digital/io-hub/src/common-cc/move.cc386
-rw-r--r--digital/io-hub/src/common-cc/move.hh88
-rw-r--r--digital/io-hub/src/common-cc/obstacles.cc42
-rw-r--r--digital/io-hub/src/common-cc/obstacles.hh22
-rw-r--r--digital/io-hub/src/common-cc/path.hh (renamed from digital/io-hub/src/apbirthday/move.cc)24
7 files changed, 565 insertions, 11 deletions
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 <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 ();
+}
+
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/apbirthday/move.cc b/digital/io-hub/src/common-cc/path.hh
index 6a1041d0..3c3cce9c 100644
--- a/digital/io-hub/src/apbirthday/move.cc
+++ b/digital/io-hub/src/common-cc/path.hh
@@ -1,3 +1,5 @@
+#ifndef path_hh
+#define path_hh
// io-hub - Modular Input/Output. {{{
//
// Copyright (C) 2013 Nicolas Schodet
@@ -21,12 +23,20 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
-//#include "move.hh"
+#include "defs.hh"
-#include "robot.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_;
+};
-FSM_EVENTS (
- // Report from asserv after a successful move command.
- robot_move_success,
- // Report from asserv after a failed move command.
- robot_move_failure)
+#endif // path_hh