fix sec enc bug, however need more test

interface_node/launch/function_lib.py

@@ -37,8 +37,8 @@ class MActPar:
     MAXVEL = 1
     MAXPOW = 450
     MAXCUR = 40 
-    KP = 5.0
-    KD = 0.05
+    KP = 0.0
+    KD = 0.0
     KI = 0
     FBV = 27.5
     RID = 9

interface_node/launch/prova_set.py

@@ -11,10 +11,10 @@ async def main():
 ######################################################################################################################################################################################
 #                                                                              SERVOS CONFIGURATION                                                                                  #
 ######################################################################################################################################################################################    
-    ids = [2,1]#[2,1,3,4]
+    ids = [1]#[2,1,3,4]
     transport = moteus_pi3hat.Pi3HatRouter(
         servo_bus_map = {
-            1:[2],2:[1]          
+            1:ids         
         }
     )
 

moteus_pi3hat/include/moteus_pi3hat/moteus_protocol.h

@@ -22,7 +22,9 @@
 #include <limits>
 #include <tuple>
 #include <stdexcept>
-
+#include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/macros.hpp"
 /// @file
 ///
 /// This describes helper classes useful for constructing and parsing
@@ -283,6 +285,7 @@ class WriteCombiner {
     if (current_resolution_ == resolutions_[this_offset]) {
       // We don't need to write any register operations here, and the
       // value should go out only if requested.
+      // RCLCPP_INFO(rclcpp::get_logger("DIO"), "Exit because all are Kignore");
       return current_resolution_ != Resolution::kIgnore;
     }
     // We need to do some kind of framing.  See how far ahead the new
@@ -300,6 +303,7 @@ class WriteCombiner {
          i < N && resolutions_[i] == new_resolution;
          i++) {
       count++;
+      // RCLCPP_INFO(rclcpp::get_logger("MMMM"),"has count %d",count);
     }
 
     int8_t write_command = base_command_ + [&]() {
@@ -326,6 +330,7 @@ class WriteCombiner {
     if ((start_register_ + this_offset) > 127) {
       throw std::logic_error("unsupported");
     }
+
     frame_->Write<int8_t>(start_register_ + this_offset);
     return true;
   }
@@ -564,6 +569,20 @@ struct PositionResolution {
 
     );
   };
+  void operator=(const PositionResolution a)
+  {
+
+      position = a.position;
+      velocity = a.velocity;
+      feedforward_torque = a.velocity;
+      kp_scale = a.kp_scale;
+      kd_scale = a.kp_scale;
+      maximum_torque = a.maximum_torque;
+      stop_position = a.stop_position;
+      watchdog_timeout = a.watchdog_timeout;
+
+
+  };
 };
 
 inline void EmitStopCommand(WriteCanFrame* frame) {
@@ -643,6 +662,7 @@ struct QueryCommand {
         temperature != Resolution::kIgnore ||
         fault != Resolution::kIgnore;
   }
+
 };
 
 // struct added to send query request also for second encoder position and velocity 
@@ -690,6 +710,21 @@ struct QueryCommandV2 {
         sec_enc_vel == a.sec_enc_vel
     );
   };
+  void operator=(const QueryCommandV2 a)
+  {
+      mode = a.mode;
+      position = a.position;
+      velocity = a.velocity;
+      torque = a.torque;
+      q_current = a.q_current;
+      d_current = a.d_current;
+      rezero_state = a.rezero_state;
+      voltage = a.voltage;
+      temperature = a.temperature;
+      fault = a.fault;
+      sec_enc_pos = a.sec_enc_pos;
+      sec_enc_vel = a.sec_enc_vel;
+  };
 };
 
 
@@ -725,6 +760,7 @@ inline void EmitQueryCommandV2(
     WriteCanFrame* frame,
     const QueryCommandV2& command) {
   {
+    // RCLCPP_INFO(rclcpp::get_logger("LIV"),"first six value");
     WriteCombiner<6> combiner(frame, 0x10, Register::kMode, {
         command.mode,
             command.position,
@@ -738,6 +774,7 @@ inline void EmitQueryCommandV2(
     }
   }
   {
+    // RCLCPP_INFO(rclcpp::get_logger("LIV"),"mix faur value");
     WriteCombiner<4> combiner(frame, 0x10, Register::kRezeroState, {
         command.rezero_state,
             command.voltage,
@@ -748,6 +785,7 @@ inline void EmitQueryCommandV2(
     }
   }
   {
+    // RCLCPP_INFO(rclcpp::get_logger("LIV"),"last two value with res %d %d",command.sec_enc_pos,command.sec_enc_vel);
     WriteCombiner<2> combiner(frame, 0x10, Register::kEncoder1Pos,{
       command.sec_enc_pos,
       command.sec_enc_vel});

moteus_pi3hat/include/moteus_pi3hat/pi3hat_moteus_interface.h

@@ -148,6 +148,14 @@ class Pi3HatMoteusInterface {
     callback_ = std::move(callback);
     active_ = true;
     data_ = data;
+    // for(auto cmd : data.commands)
+    // {
+    //   RCLCPP_INFO(rclcpp::get_logger("LIV3"),"value of data is %d",cmd.query.sec_enc_pos);
+    // }
+    //  for(auto cmd : data_.commands)
+    // {
+    //   RCLCPP_INFO(rclcpp::get_logger("LIV4"),"value of data is %d",cmd.query.sec_enc_pos);
+    // }
     }
     condition_.notify_one();
     // RCLCPP_INFO(rclcpp::get_logger("CYCLE"),"PASS");
@@ -227,6 +235,7 @@ class Pi3HatMoteusInterface {
           throw std::logic_error("unsupported mode");
         }
       }
+      // RCLCPP_INFO(rclcpp::get_logger("LIV2"),"the command value is %d and %d",cmd.query.sec_enc_pos,cmd.query.sec_enc_vel);
       moteus::EmitQueryCommandV2(&write_frame, cmd.query);
     }
 

pi3hat_hw_interface/config/test_config.yaml

@@ -8,17 +8,19 @@ controller_manager:
 state_broadcaster:
   ros__parameters:
     joints:
-      - jnt_b9_1Hz_trq4_c2
-      - jnt_b9_1Hz_trq4_c3
+      - joint
+      # - jnt_b9_1Hz_trq4_c2
+      # - jnt_b9_1Hz_trq4_c3
       # - joinjoint1t3
       # - joint4
     performance_index: true
 joint_controller:
   ros__parameters:
     joints:
-      - jnt_b9_1Hz_trq4_c2
-      - jnt_b9_1Hz_trq4_c3
-      # - joint3
+      - joint
+      # - jnt_b9_1Hz_trq4_c2
+      # - jnt_b9_1Hz_trq4_c3
+      # - joinjoint1t3
       # - joint4
 
 

pi3hat_hw_interface/include/pi3hat_hw_interface/moteus_pi3hat_interface.hpp

@@ -90,10 +90,14 @@ namespace pi3hat_hw_interface
                             rep.result.velocity = std::nan("2");
                             rep.result.torque = std::nan("3");
                             rep.result.temperature = std::nan("4");
-                            rep.result.sec_enc_pos = std::nan("1");
+                            rep.result.sec_enc_pos = std::nan("6");
                             rep.result.sec_enc_vel = std::nan("2");
                         }
                         data_.commands = {cmd_data_.data(),cmd_data_.size()};
+                        //  for(auto cmd : data_.commands)
+                        //     {
+                        //     RCLCPP_INFO(rclcpp::get_logger("LIV10"),"value of data is %d",cmd.query.sec_enc_pos);
+                        //     }
                         data_.replies  = {msr_data_.data(),msr_data_.size()};
                         communication_thread_.Cycle(
                                 data_,

pi3hat_hw_interface/include/pi3hat_hw_interface/motor_manager.hpp

@@ -122,30 +122,9 @@ namespace pi3hat_hw_interface
                             hardware_interface::HW_IF_KP_SCALE,
                             hardware_interface::HW_IF_KD_SCALE
                         };
-                    if(second_trans == 0.0)
-                    {
-
-                        inter_type_stt_ = {
-                            hardware_interface::HW_IF_POSITION,
-                            hardware_interface::HW_IF_VELOCITY,
-                            hardware_interface::HW_IF_EFFORT,
-                            hardware_interface::HW_IF_TEMPERATURE,
-                            hardware_interface::HW_IF_PACKAGE_LOSS
-
-                        };
-                    }
-                    else
-                    {
-                        inter_type_stt_ = {
-                            hardware_interface::HW_IF_POSITION,
-                            hardware_interface::HW_IF_VELOCITY,
-                            hardware_interface::HW_IF_EFFORT,
-                            hardware_interface::HW_IF_TEMPERATURE,
-                            hardware_interface::HW_IF_PACKAGE_LOSS,
-                            hardware_interface::HW_IF_POSITION,
-                            hardware_interface::HW_IF_VELOCITY
-                        };
-                    }
+                    inter_type_stt_ = {};
+                    // RCLCPP_INFO(rclcpp::get_logger("DIO"),"pass sec_trans %f",second_trans);
+
                     //std::function<void(bool,bool,Command&)> a = bind(&pl_fun,this, _1,_2,_3);
                 };
                 ~Motor_Manager()
@@ -171,7 +150,31 @@ namespace pi3hat_hw_interface
                         bus_ = bus;
                         get_callback_ = gt_fun;
                         pol_callback_ = std::bind(pl_fun,_1,_2,this->cmd_data_);
-
+                         if(second_trans == 0.0)
+                        {
+
+                            inter_type_stt_ = {
+                                hardware_interface::HW_IF_POSITION,
+                                hardware_interface::HW_IF_VELOCITY,
+                                hardware_interface::HW_IF_EFFORT,
+                                hardware_interface::HW_IF_TEMPERATURE,
+                                hardware_interface::HW_IF_PACKAGE_LOSS
+
+                            };
+                        }
+                        else
+                        {
+                            inter_type_stt_ = {
+                                hardware_interface::HW_IF_POSITION,
+                                hardware_interface::HW_IF_VELOCITY,
+                                hardware_interface::HW_IF_EFFORT,
+                                hardware_interface::HW_IF_TEMPERATURE,
+                                hardware_interface::HW_IF_PACKAGE_LOSS,
+                                hardware_interface::HW_IF_POSITION,
+                                hardware_interface::HW_IF_VELOCITY
+                            };
+
+                        }
                     };
                 // set and get the current command resolution
                 void set_command_resolution(moteus::PositionResolution res);
@@ -359,7 +362,7 @@ namespace pi3hat_hw_interface
                 int packet_loss_ = 0;
                 double loss_var_ = 0.0;
                 double sec_enc_off_ = 0.0, old_sec_enc_ = 0.0;
-                bool first_read_ = false;
+                bool first_read_ = true;
                 int sec_enc_counter_ = 0;
 
 

pi3hat_hw_interface/src/moteus_pi3hat_interface.cpp

@@ -46,7 +46,7 @@ namespace pi3hat_hw_interface
                             else
                             {
                                 err = 0;
-
+                                // RCLCPP_INFO(rclcpp::get_logger("PASS"),"sec enc has value %f",item.result.sec_enc_pos);
 
                                 return item.result; 
                                 //std::printf("FOUND: %d,%d",item.id,item.bus);
@@ -144,6 +144,7 @@ namespace pi3hat_hw_interface
             }
             for(auto joint :info.joints)
             {
+                // RCLCPP_INFO(rclcpp::get_logger("PORCO"),"mt: %f and srt: %f",std::stod(joint.parameters.at("motor_transmission")),std::stod(joint.parameters.at("sec_enc_transmission")));
                 id = static_cast<uint8_t>(std::stoi(joint.parameters.at("id")));
 
                 bus =static_cast<uint8_t>(std::stoi(joint.parameters.at("bus")));
@@ -202,7 +203,7 @@ namespace pi3hat_hw_interface
             PQ.temperature = moteus::Resolution::kFloat;
             PQ.fault = moteus::Resolution::kInt8;
 
-            for(auto motor : motors_)
+            for(auto &motor : motors_)
             {
                 motor.set_command_resolution(PC);
                 if(motor.get_sec_transmission() == 0.0)
@@ -216,6 +217,9 @@ namespace pi3hat_hw_interface
                     PQ.sec_enc_vel = moteus::Resolution::kFloat;
                 }
                 motor.set_query_resolution(PQ);
+                // RCLCPP_INFO(rclcpp::get_logger("TEST___"),"Quesy resolution encoder is %d %d",PQ.sec_enc_pos,PQ.sec_enc_vel);
+                // auto pp = motor.get_qry_res();
+                // RCLCPP_INFO(rclcpp::get_logger("TEST___TEST"),"Quesy resolution encoder is %d %d",pp.sec_enc_pos,pp.sec_enc_vel);
             } 
             communication_thread_.start_communication();
 
@@ -279,7 +283,12 @@ namespace pi3hat_hw_interface
                 {
                     motor.make_stop();
                 }  
+                // for(auto cmd:data_.commands)
+                // {
 
+                //     RCLCPP_INFO(rclcpp::get_logger("LIV20"),"value of data is %d",cmd.query.sec_enc_pos);
+
+                // }
                 // RCLCPP_INFO(rclcpp::get_logger("LOGGER_NAME"),"Stop command %d",i);
 
                 // rclcpp::sleep_for(a);
@@ -472,13 +481,15 @@ namespace pi3hat_hw_interface
 
         hardware_interface::return_type MoteusPi3Hat_Interface::write(const rclcpp::Time & , const rclcpp::Duration & ) 
         {
+
             if(valid_)
             {
                 for(auto &motor : motors_)
                 {
                     motor.make_position();
                     // RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "the motor ID is %d",motor.get_id());
-
+                    // auto pp = motor.get_qry_res();
+                    // RCLCPP_INFO(rclcpp::get_logger("TEST___TEST4"),"Quesy resolution encoder is %d %d",pp.sec_enc_pos,pp.sec_enc_vel);
                 }
 
                 // int i  = 0;