From bbd0d642484b2865b8290e70e5b7c4ba45a41856 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 8 Apr 2013 22:14:52 +0200 Subject: digital/io-hub/src/apbirthday: change Asserv::get_position syntax --- digital/io-hub/src/apbirthday/robot.cc | 6 ++---- digital/io-hub/src/apbirthday/top.cc | 6 ++---- digital/io-hub/src/common-cc/asserv.cc | 20 ++++++++------------ digital/io-hub/src/common-cc/asserv.hh | 11 ++++++++--- digital/io-hub/src/common-cc/move.cc | 15 +++++---------- 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,22 +42,16 @@ 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 () { 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); -- cgit v1.2.3