From 4e67c3e2e763eab19d253fad78331a0b4ac6625a Mon Sep 17 00:00:00 2001 From: Olivier Lanneluc Date: Thu, 18 Apr 2013 02:59:36 +0200 Subject: digital/io-hub/src/common-cc: allow robot to target an obstacle (the cake) --- digital/io-hub/src/common-cc/path.cc | 78 ++++++++++++++++++++++++------------ 1 file changed, 52 insertions(+), 26 deletions(-) (limited to 'digital/io-hub/src/common-cc/path.cc') diff --git a/digital/io-hub/src/common-cc/path.cc b/digital/io-hub/src/common-cc/path.cc index 6febd082..8d709c9a 100644 --- a/digital/io-hub/src/common-cc/path.cc +++ b/digital/io-hub/src/common-cc/path.cc @@ -28,7 +28,6 @@ #include "bot.hh" #include "robot.hh" #include "playground.hh" -#include "playground_2013.hh" extern "C" { #include "modules/math/fixed/fixed.h" @@ -47,6 +46,10 @@ extern "C" { /** Angle between obstacles points. */ #define PATH_ANGLE_824(pOINTS_NB) ((1L << 24) / (pOINTS_NB)) +/** Check for vectors equality */ +#define PATH_VECT_EQUAL(v1, v2) ((v1)->x==(v2)->x && (v1)->y==(v2)->y) + +/** Static nodes index for the endpoints */ enum { PATH_NAVPOINT_SRC_IDX = 0, PATH_NAVPOINT_DST_IDX @@ -78,8 +81,10 @@ void Path::reset() next_node = 0; escape_factor = 0; +#ifdef playground_2013_hh /* Declare the cake as an obstacle */ add_obstacle(pg_cake_pos, pg_cake_radius, PATH_CAKE_NAVPOINTS_NB); +#endif } void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) @@ -148,11 +153,6 @@ void Path::add_obstacle(const vect_t &c, uint16_t r, const int nodes) #ifdef HOST /* Plot obstacle points */ robot->hardware.simu_report.pos( &navpoints[PATH_RESERVED_NAVPOINTS_NB], navpoints_nb-PATH_RESERVED_NAVPOINTS_NB, PATH_PLOT_ID); -#if 0 - /* Draw the last obstacle */ - navpoints[navpoints_nb] = navpoints[navpoints_nb - num]; - robot->hardware.simu_report.path( &navpoints[navpoints_nb - num], num + 1); -#endif #endif } @@ -175,27 +175,37 @@ int Path::find_neighbors(int cur_point, struct astar_neighbor_t *neighbors) for(int j=0; j validated w=%u\n", weight); @@ -261,14 +271,30 @@ void Path::obstacle(int index, const vect_t &c, uint16_t r, int f) add_obstacle(c, r, PATH_OBSTACLES_NAVPOINTS_NB); } -void Path::endpoints(const vect_t &src, const vect_t &dst) +void Path::endpoints(const vect_t &src, const vect_t &dst, const bool force_move) { + /* Store endpoints location */ DPRINTF("Set path endpoints src=(%u;%u) dst=(%u;%u)\n", src.x, src.y, dst.x, dst.y); navpoints[PATH_NAVPOINT_SRC_IDX] = src; navpoints[PATH_NAVPOINT_DST_IDX] = dst; + + /* Init endpoints weights */ navweights[PATH_NAVPOINT_SRC_IDX] = (1<force_move = force_move; + +#ifdef playground_2013_hh + /* Temporary code for the cake */ + if (PATH_VECT_EQUAL(&dst, &pg_cake_pos)) + { + this->force_move = true; + } +#endif } bool Path::get_next(vect_t &p) -- cgit v1.2.3