add second encoder counter and offset

CentroEPiaggio · Sep 8, 2023 · 6ee3890 · 6ee3890
1 parent c6b1830
commit 6ee3890
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 29 deletions.
diff --git 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
@@ -40,6 +40,7 @@ namespace pi3hat_joint_group_controller
 
         }
         default_init_pos_ = true;
+        joints_rcvd_msg_ = std::make_shared<CmdMsgs>();
         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
@@ -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
@@ -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<double>(loss_var_ * cnt_lim * ep_cnt + packet_loss_ ) / 
-                    static_cast<double>(cnt_lim * (ep_cnt + 1));
+                    loss_var_ = packet_loss_;
+                    // static_cast<double>(loss_var_ * cnt_lim * ep_cnt + packet_loss_ ) / 
+                    // static_cast<double>(cnt_lim * (ep_cnt + 1));
                     packet_loss_ = 0;
                 };
                 bool get_msg_arrived()
@@ -357,6 +358,9 @@ namespace pi3hat_hw_interface
                 std::vector<std::string> 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
@@ -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<double>(not_val_cycle_))/static_cast<double>((epoch_count_ +1 )*MAX_COUNT);
+                valid_loss_ = not_val_cycle_; //(perc + static_cast<double>(not_val_cycle_))/static_cast<double>((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
@@ -3,10 +3,11 @@
 #include "moteus_pi3hat/pi3hat.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/macros.hpp"
-
 #include <stdio.h>
 #include <cstdio>
 
+#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
@@ -3,12 +3,12 @@
     <ros2_control name="MoteusPi3Hat_Interface" type="system">
         <hardware>
             <plugin>pi3hat_hw_interface/MoteusPi3Hat_Interface</plugin>
-            <param name="main_timeout">100</param>
+            <param name="main_timeout">20000</param>
             <param name="can_timeout">20000</param>
             <param name="rcv_timeout">20000</param>
             <param name="attitude">0</param>
         </hardware>
-         <!-- <joint name="joint1">
+         <!-- <joint name="joint3">
             <param name="id">2</param>
             <param name="bus">1</param>
             <param name="motor_transmission"> 1.0</param>
@@ -19,18 +19,18 @@
             <param name="bus">1</param>
             <param name="motor_transmission"> 1.0</param>
             <param name="sec_enc_transmission"> 0.0</param>
-        </joint> 
-        <joint name="joint3">
-            <param name="id">3</param>
-            <param name="bus">1</param>
-            <param name="motor_transmission"> 1.0</param>
+        </joint>  -->
+        <joint name="jnt_b9_1Hz_trq4_c2">
+            <param name="id">1</param>
+            <param name="bus">2</param>
+            <param name="motor_transmission"> 9.0</param>
             <param name="sec_enc_transmission"> 0.0</param>
-        </joint> -->
+        </joint>
 
-        <joint name="joint">
-            <param name="id">4</param>
+        <joint name="jnt_b9_1Hz_trq4_c3">
+            <param name="id">2</param>
             <param name="bus">1</param>
-            <param name="motor_transmission"> 1.0</param>
+            <param name="motor_transmission"> 9.0</param>
             <param name="sec_enc_transmission"> 0.0</param>
         </joint>