From 6ee3890407a679618c010403782816580e45ddf3 Mon Sep 17 00:00:00 2001 From: Cionix90 Date: Fri, 8 Sep 2023 07:55:03 +0000 Subject: [PATCH] add second encoder counter and offset --- interface_node/launch/prova_set.py | 6 ++--- .../src/pi3hat_joint_group_controller.cpp | 13 ++++++++--- pi3hat_hw_interface/config/test_config.yaml | 10 ++++----- .../pi3hat_hw_interface/motor_manager.hpp | 10 ++++++--- .../src/moteus_pi3hat_interface.cpp | 4 ++-- pi3hat_hw_interface/src/motor_manager.cpp | 15 +++++++++++-- pi3hat_hw_interface/urdf/test_int.urdf.xacro | 22 +++++++++---------- 7 files changed, 51 insertions(+), 29 deletions(-) diff --git a/interface_node/launch/prova_set.py b/interface_node/launch/prova_set.py index 04e8559..9c2f3da 100644 --- a/interface_node/launch/prova_set.py +++ b/interface_node/launch/prova_set.py @@ -11,10 +11,10 @@ async def main(): ###################################################################################################################################################################################### # SERVOS CONFIGURATION # ###################################################################################################################################################################################### - ids = [4]#[2,1,3,4] + ids = [2,1]#[2,1,3,4] transport = moteus_pi3hat.Pi3HatRouter( servo_bus_map = { - 1:ids + 1:[2],2:[1] } ) @@ -23,7 +23,7 @@ async def main(): servos ={id: moteus.Controller(id=id, transport=transport, query_resolution = qr) for id in ids} - + for id in ids: print(f"execute stop id {id}") diff --git a/pi3hat_base_controller/src/pi3hat_joint_group_controller.cpp b/pi3hat_base_controller/src/pi3hat_joint_group_controller.cpp index 8e1e8c6..743735b 100644 --- a/pi3hat_base_controller/src/pi3hat_joint_group_controller.cpp +++ b/pi3hat_base_controller/src/pi3hat_joint_group_controller.cpp @@ -40,6 +40,7 @@ namespace pi3hat_joint_group_controller } default_init_pos_ = true; + joints_rcvd_msg_ = std::make_shared(); RCLCPP_INFO(get_node()->get_logger(),"initialize succesfully"); return CallbackReturn::SUCCESS; } @@ -93,7 +94,13 @@ namespace pi3hat_joint_group_controller 5, [this](const CmdMsgs::SharedPtr msg) { - rt_buffer_.writeFromNonRT(msg); + // rt_buffer_.writeFromNonRT(msg); + joints_rcvd_msg_->set__name(msg->name); + joints_rcvd_msg_->set__position(msg->position); + joints_rcvd_msg_->set__velocity(msg->velocity); + joints_rcvd_msg_->set__effort(msg->effort); + joints_rcvd_msg_->set__kp_scale(msg->kp_scale); + joints_rcvd_msg_->set__kd_scale(msg->kd_scale); } ); RCLCPP_INFO(get_node()->get_logger(),"configure succesfully"); @@ -119,7 +126,7 @@ namespace pi3hat_joint_group_controller bool Pi3Hat_Joint_Group_Controller::get_reference() { - joints_rcvd_msg_ = *rt_buffer_.readFromRT(); + // joints_rcvd_msg_ = *rt_buffer_.readFromRT(); if(joints_rcvd_msg_.get()) { @@ -158,7 +165,7 @@ namespace pi3hat_joint_group_controller } } else - RCLCPP_WARN(rclcpp::get_logger(logger_name_),"No data are readed from realtime buffer"); + // RCLCPP_WARN(rclcpp::get_logger(logger_name_),"No data are readed from realtime buffer"); // maybe needed? //joints_rcvd_msg_.reset(); diff --git a/pi3hat_hw_interface/config/test_config.yaml b/pi3hat_hw_interface/config/test_config.yaml index 7b0a8f6..7c2e51b 100644 --- a/pi3hat_hw_interface/config/test_config.yaml +++ b/pi3hat_hw_interface/config/test_config.yaml @@ -8,16 +8,16 @@ controller_manager: state_broadcaster: ros__parameters: joints: - - joint - # - joint2 - # - joint3 + - jnt_b9_1Hz_trq4_c2 + - jnt_b9_1Hz_trq4_c3 + # - joinjoint1t3 # - joint4 performance_index: true joint_controller: ros__parameters: joints: - - joint - # - joint2 + - jnt_b9_1Hz_trq4_c2 + - jnt_b9_1Hz_trq4_c3 # - joint3 # - joint4 diff --git a/pi3hat_hw_interface/include/pi3hat_hw_interface/motor_manager.hpp b/pi3hat_hw_interface/include/pi3hat_hw_interface/motor_manager.hpp index 4489e3a..36b120d 100644 --- a/pi3hat_hw_interface/include/pi3hat_hw_interface/motor_manager.hpp +++ b/pi3hat_hw_interface/include/pi3hat_hw_interface/motor_manager.hpp @@ -298,10 +298,11 @@ namespace pi3hat_hw_interface return loss_var_; }; - void reset_pkg_loss(int cnt_lim, int ep_cnt) + void reset_pkg_loss()//(int cnt_lim, int ep_cnt) { - loss_var_ = static_cast(loss_var_ * cnt_lim * ep_cnt + packet_loss_ ) / - static_cast(cnt_lim * (ep_cnt + 1)); + loss_var_ = packet_loss_; + // static_cast(loss_var_ * cnt_lim * ep_cnt + packet_loss_ ) / + // static_cast(cnt_lim * (ep_cnt + 1)); packet_loss_ = 0; }; bool get_msg_arrived() @@ -357,6 +358,9 @@ namespace pi3hat_hw_interface std::vector inter_type_cmd_; int packet_loss_ = 0; double loss_var_ = 0.0; + double sec_enc_off_ = 0.0, old_sec_enc_ = 0.0; + bool first_read_ = false; + int sec_enc_counter_ = 0; diff --git a/pi3hat_hw_interface/src/moteus_pi3hat_interface.cpp b/pi3hat_hw_interface/src/moteus_pi3hat_interface.cpp index 5d107d8..ca72778 100644 --- a/pi3hat_hw_interface/src/moteus_pi3hat_interface.cpp +++ b/pi3hat_hw_interface/src/moteus_pi3hat_interface.cpp @@ -437,7 +437,7 @@ namespace pi3hat_hw_interface // ,motors_[j].get_id(),perc); // epoch_count_++; // lost_pkt = motors_[j].get_pkg_loss(); - motors_[j].reset_pkg_loss(MAX_COUNT,epoch_count_); + motors_[j].reset_pkg_loss();//(MAX_COUNT,epoch_count_); // RCLCPP_WARN( // rclcpp::get_logger(LOGGER_NAME), @@ -454,7 +454,7 @@ namespace pi3hat_hw_interface perc = valid_loss_ * MAX_COUNT * (epoch_count_ ); - valid_loss_ = (perc + static_cast(not_val_cycle_))/static_cast((epoch_count_ +1 )*MAX_COUNT); + valid_loss_ = not_val_cycle_; //(perc + static_cast(not_val_cycle_))/static_cast((epoch_count_ +1 )*MAX_COUNT); epoch_count_++; // RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Not valid msg count is %d",not_val_cycle_); count_ = 0; diff --git a/pi3hat_hw_interface/src/motor_manager.cpp b/pi3hat_hw_interface/src/motor_manager.cpp index 7543a29..e4b435e 100644 --- a/pi3hat_hw_interface/src/motor_manager.cpp +++ b/pi3hat_hw_interface/src/motor_manager.cpp @@ -3,10 +3,11 @@ #include "moteus_pi3hat/pi3hat.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp/macros.hpp" - #include #include +#define DELTA 0.5 + namespace pi3hat_hw_interface { namespace motor_manager @@ -153,6 +154,7 @@ namespace pi3hat_hw_interface int Motor_Manager::get_motor_state(int prov_msg) { int error; + double diff; // count_++; // if(count_ % MAX_COUNT == 0) // { @@ -190,10 +192,19 @@ namespace pi3hat_hw_interface msr_vel_ = res.velocity/motor_trans_; msr_trq_ = res.torque*motor_trans_; msr_tmp_ = res.temperature; + if(sec_enc_trans_ != 0.0) { - msr_enc_pos_ = res.sec_enc_pos/sec_enc_trans_; + if(!first_read_) + sec_enc_off_ = res.sec_enc_pos/sec_enc_trans_; + msr_enc_pos_ = res.sec_enc_pos/sec_enc_trans_ - sec_enc_off_; msr_enc_vel_ = res.sec_enc_vel/sec_enc_trans_; + diff = msr_enc_pos_ - old_sec_enc_; + if(diff > 0 && std::abs(diff) > DELTA) + sec_enc_counter_ --; + else if (diff < 0 && std::abs(diff) > DELTA) + sec_enc_counter_ ++; + msr_enc_pos_ += (float)sec_enc_counter_; } return res.fault; diff --git a/pi3hat_hw_interface/urdf/test_int.urdf.xacro b/pi3hat_hw_interface/urdf/test_int.urdf.xacro index 27ca623..20b110a 100644 --- a/pi3hat_hw_interface/urdf/test_int.urdf.xacro +++ b/pi3hat_hw_interface/urdf/test_int.urdf.xacro @@ -3,12 +3,12 @@ pi3hat_hw_interface/MoteusPi3Hat_Interface - 100 + 20000 20000 20000 0 - + + 1 + 2 + 9.0 0.0 - --> + - - 4 + + 2 1 - 1.0 + 9.0 0.0