summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-08 22:14:52 +0200
committerNicolas Schodet2013-04-10 00:43:00 +0200
commitbbd0d642484b2865b8290e70e5b7c4ba45a41856 (patch)
treed405f4fb211c6986da45213e6a643d3e7729aa36 /digital
parentbcaf3117b0e0c7c4dc8bf5442637d2639a92989a (diff)
digital/io-hub/src/apbirthday: change Asserv::get_position syntax
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc6
-rw-r--r--digital/io-hub/src/apbirthday/top.cc6
-rw-r--r--digital/io-hub/src/common-cc/asserv.cc20
-rw-r--r--digital/io-hub/src/common-cc/asserv.hh11
-rw-r--r--digital/io-hub/src/common-cc/move.cc15
5 files changed, 25 insertions, 33 deletions
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index d6f29964..cd782ffc 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -124,8 +124,7 @@ Robot::main_loop ()
// Handle communications.
bool sync = main_i2c_queue_.sync ();
zb_i2c_queue_.sync ();
- Position robot_pos;
- asserv.get_position (robot_pos);
+ Position robot_pos = asserv.get_position ();
beacon.send_position (robot_pos.v);
// Look for obstacles.
if (usdist_control_.update ())
@@ -336,8 +335,7 @@ Robot::proto_stats ()
return;
if (stats_asserv_ && !--stats_asserv_cpt_)
{
- Position pos;
- asserv.get_position (pos);
+ Position pos = asserv.get_position ();
stats_proto_->send ('A', "hhH", pos.v.x, pos.v.y, pos.a);
stats_asserv_cpt_ = stats_asserv_;
}
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index 46c55a29..0f3f60f5 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -64,8 +64,7 @@ top_cake_angle (const vect_t &pos)
static uint16_t
top_cake_angle_robot ()
{
- Position pos;
- robot->asserv.get_position (pos);
+ Position pos = robot->asserv.get_position ();
return top_cake_angle (pos.v);
}
@@ -142,8 +141,7 @@ top_follow_or_leave ()
bool
top_follow_blocking (int dir_sign)
{
- Position robot_pos;
- robot->asserv.get_position (robot_pos);
+ Position robot_pos = robot->asserv.get_position ();
uint16_t robot_angle = top_cake_angle (robot_pos.v);
// Check for an obstacle on a small segment.
vect_t dst;
diff --git a/digital/io-hub/src/common-cc/asserv.cc b/digital/io-hub/src/common-cc/asserv.cc
index 505c2a6a..acffda99 100644
--- a/digital/io-hub/src/common-cc/asserv.cc
+++ b/digital/io-hub/src/common-cc/asserv.cc
@@ -29,10 +29,12 @@ Asserv::Asserv (I2cQueue &queue, float scale)
: I2cQueue::Slave (queue, 0x04, 10 + aux_nb * 2),
status_flag_ (0),
input_port_ (0),
- position_x_ (0), position_y_ (0), position_a_ (0),
last_moving_direction_ (DIRECTION_NONE),
scale_ (scale), scale_inv_ (1 / scale)
{
+ position_.v.x = 0;
+ position_.v.y = 0;
+ position_.a = 0;
}
void
@@ -40,23 +42,17 @@ Asserv::recv_status (const uint8_t *status)
{
status_flag_ = status[0];
input_port_ = status[1];
- position_x_ = ucoo::bytes_pack (0, status[2], status[3], status[4]);
- position_y_ = ucoo::bytes_pack (0, status[5], status[6], status[7]);
- position_a_ = ucoo::bytes_pack (status[8], status[9]);
+ position_.v.x = ucoo::bytes_pack (0, status[2], status[3], status[4])
+ * scale_;
+ position_.v.y = ucoo::bytes_pack (0, status[5], status[6], status[7])
+ * scale_;
+ position_.a = ucoo::bytes_pack (status[8], status[9]);
Direction d = get_moving_direction ();
if (d != DIRECTION_NONE)
last_moving_direction_ = d;
}
void
-Asserv::get_position (Position &position) const
-{
- position.v.x = position_x_ * scale_;
- position.v.y = position_y_ * scale_;
- position.a = position_a_;
-}
-
-void
Asserv::free ()
{
uint8_t buf[] = { 'w' };
diff --git a/digital/io-hub/src/common-cc/asserv.hh b/digital/io-hub/src/common-cc/asserv.hh
index ea6f5ec4..07d5f8ef 100644
--- a/digital/io-hub/src/common-cc/asserv.hh
+++ b/digital/io-hub/src/common-cc/asserv.hh
@@ -58,7 +58,7 @@ class Asserv : public I2cQueue::Slave
/// Get last moving direction.
Direction get_last_moving_direction () const;
/// Get current position.
- void get_position (Position &position) const;
+ const Position &get_position () const;
/// Release motors (zero torque).
void free ();
/// Stop moving, applying acceleration constraints.
@@ -91,8 +91,7 @@ class Asserv : public I2cQueue::Slave
private:
uint8_t status_flag_;
uint8_t input_port_;
- int position_x_, position_y_;
- uint16_t position_a_;
+ Position position_;
Direction last_moving_direction_;
float scale_, scale_inv_;
};
@@ -125,4 +124,10 @@ Asserv::get_last_moving_direction () const
return last_moving_direction_;
}
+inline const Position &
+Asserv::get_position () const
+{
+ return position_;
+}
+
#endif // asserv_hh
diff --git a/digital/io-hub/src/common-cc/move.cc b/digital/io-hub/src/common-cc/move.cc
index a4d490bc..bdbd3da0 100644
--- a/digital/io-hub/src/common-cc/move.cc
+++ b/digital/io-hub/src/common-cc/move.cc
@@ -69,8 +69,7 @@ Move::check_obstacles ()
{
if (FSM_CAN_HANDLE (AI, obstacle_blocking))
{
- Position robot_pos;
- robot->asserv.get_position (robot_pos);
+ Position robot_pos = robot->asserv.get_position ();
if (robot->obstacles.blocking (robot_pos.v, step_))
if (FSM_HANDLE (AI, obstacle_blocking))
return true;
@@ -116,8 +115,7 @@ Move::go ()
if (step_final_move_ && shorten_)
{
// Compute a vector from destination to robot with lenght 'shorten'.
- Position robot_position;
- robot->asserv.get_position (robot_position);
+ Position robot_position = robot->asserv.get_position ();
vect_t v = robot_position.v;
vect_sub (&v, &step_);
int d = vect_norm (&v);
@@ -140,8 +138,7 @@ 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);
+ Position robot_position = robot->asserv.get_position ();
uint16_t robot_angle = robot_position.a;
if (direction_consign & Asserv::BACKWARD)
robot_angle += 0x8000;
@@ -205,8 +202,7 @@ Move::path_init ()
bool found;
vect_t dst;
// Get the current position
- Position current_pos;
- robot->asserv.get_position (current_pos);
+ Position current_pos = robot->asserv.get_position ();
// Give the current position of the bot to the path module
robot->path.reset ();
robot->obstacles.add_obstacles (robot->path);
@@ -320,8 +316,7 @@ move_back_up (void)
back_dist = 300;
}
// Assume there is an obstacle in front of the robot.
- Position robot_pos;
- robot->asserv.get_position (robot_pos);
+ Position robot_pos = robot->asserv.get_position ();
vect_t obstacle_pos;
vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a);
vect_translate (&obstacle_pos, &robot_pos.v);