dm motor kp bug fix

Author: Raventhatfly

decomposition/metav_description/urdf/playground/dm_motor.xacro

@@ -38,22 +38,22 @@
             <hardware>
                 <plugin>meta_hardware/MetaRobotDmMotorNetwork</plugin>
                 <param name="can_network_name">can0</param>
-                <param name="master_id">0x30</param>
+                <param name="master_id">0x00</param>
             </hardware>
             <joint name="motor_joint">
                 <command_interface name="position" />
                 <state_interface name="position" />
                 <state_interface name="velocity" />
                 <state_interface name="effort" />
                 <param name="motor_model">DaMiao</param>
-                <param name="motor_id">2</param>
+                <param name="motor_id">1</param>
                 <param name="mechanical_reduction">1.0</param>
                 <param name="max_pos">3.141593</param>
                 <param name="max_vel">30</param>
                 <param name="max_effort">10</param>
                 <param name="offset">0.0</param>
                 <param name="control_mode">mit</param>
-                <param name="Kp">150.0</param>
+                <param name="Kp">30.0</param>
                 <param name="Kd">0.5</param>
             </joint>
         </ros2_control>

execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp

@@ -63,7 +63,7 @@ class DmMotor {
 
     double Kp_;
     double Kd_;
-    uint8_t Kp_raw_;
+    uint16_t Kp_raw_;
     uint16_t Kd_raw_;
 
     // Motor feedback

execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp

@@ -39,7 +39,7 @@ DmMotor::DmMotor(const std::unordered_map<std::string, std::string> &motor_param
         Kp_ = std::stod(motor_param.at("Kp"));
         Kd_ = std::stod(motor_param.at("Kd"));
 
-        Kp_raw_ = static_cast<uint8_t>((Kp_ - MIN_KP)/(MAX_KP - MIN_KP) * ( (1 << 8) - 1));
+        Kp_raw_ = static_cast<uint16_t>((Kp_ - MIN_KP)/(MAX_KP - MIN_KP) * ( (1 << 12) - 1));
         Kd_raw_ = static_cast<uint16_t>((Kd_ - MIN_KD)/(MAX_KD - MIN_KD) * ( (1 << 12) - 1));
 
         run_mode_ = RunMode::MIT;