修改了balance和trot逻辑

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -28,6 +28,7 @@ add_executable(mdog_simple_controller
   src/common/kinematics.cpp
   src/common/user_math.cpp
   src/common/bezier_curve.cpp
+  src/common/pid.cpp
 )
 ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )
 

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+namespace mdog_simple_controller {
+
+class PID {
+public:
+    PID(double kp, double ki, double kd, double dt, double max_out = 1e6, double min_out = -1e6);
+
+    void set_gains(double kp, double ki, double kd);
+    void set_limits(double min_out, double max_out);
+    void reset();
+    double compute(double setpoint, double measurement);
+
+private:
+    double kp_, ki_, kd_;
+    double dt_;
+    double max_out_, min_out_;
+    double prev_error_;
+    double integral_;
+};
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp

@@ -1,78 +1,15 @@
 #include "mdog_simple_controller/FSM/state_balance.hpp"
 #include "mdog_simple_controller/mdog_simple_controller.hpp"
-#include "mdog_simple_controller/common/user_math.hpp"
-#include "mdog_simple_controller/common/kinematics.hpp"
-#include "mdog_simple_controller/common/bezier_curve.hpp"
-#include <cmath>
 
 namespace mdog_simple_controller {
 
-namespace {
-double bezier_time = 0.0;
-const double bezier_period = 1.0;
-}
-
 void StateBalance::enter(MdogSimpleController* ctrl) {
     // 进入BALANCE状态时的初始化
 }
 
 void StateBalance::run(MdogSimpleController* ctrl) {
     // 贝塞尔控制点
-    std::vector<std::array<double, 3>> control_points = {
-        {0.30, 0.0, 0.15},      // 起点
-        {0.10, 0.0, 0.0},     // 抬腿中点
-        {0.30, 0.0, -0.15},     // 落点
-    };
-    // 起点和终点
-    const auto& p0 = control_points.front();
-    const auto& p1 = control_points.back();
 
-    // 时间推进
-    static rclcpp::Time last_time = ctrl->now();
-    rclcpp::Time now = ctrl->now();
-    double dt = (now - last_time).seconds();
-    last_time = now;
-    bezier_time += dt;
-    if (bezier_time > bezier_period) bezier_time -= bezier_period;
-    double t = bezier_time / bezier_period; // [0,1]
-
-    // 对角腿同步，交错运动，相位差0.5
-    for (int leg = 0; leg < 4; ++leg) {
-        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
-        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
-        double phase = std::fmod(t + phase_offset, 1.0);
-
-        std::array<double, 3> foot_pos;
-        if (phase < 0.5) {
-            // 摆动相：贝塞尔曲线
-            double swing_t = phase / 0.5; // 归一化到[0,1]
-            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
-        } else {
-            // 支撑相：直线连接首尾
-            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
-            for (int i = 0; i < 3; ++i) {
-                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
-            }
-        }
-
-        std::array<double, 3> theta;
-        std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
-        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
-        if (ok) {
-            // 左右腿hip方向相反，FR(0) RR(2)为右腿，FL(1) RL(3)为左腿
-            if (leg == 1 || leg == 3) { // 右腿
-                theta[0] = -theta[0];
-            }
-            for (int j = 0; j < 3; ++j) {
-                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
-                ctrl->data_.command_real[leg].speed_des[j] = 0;
-                ctrl->data_.command_real[leg].torque_des[j] = 0;
-                ctrl->data_.command_real[leg].kp[j] = 0.8;
-                ctrl->data_.command_real[leg].kd[j] = 0.01;
-            }
-        }
-    }
-    realcmd_to_cmd(ctrl->data_, ctrl->data_);
 }
 
 void StateBalance::exit(MdogSimpleController* ctrl) {

src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp

@@ -1,23 +1,78 @@
 #include "mdog_simple_controller/FSM/state_troting.hpp"
 #include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
 
 namespace mdog_simple_controller {
 
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 1.0;
+}
+
 void StateTroting::enter(MdogSimpleController* ctrl) {
     // 进入TROTING状态时的初始化
 }
 
 void StateTroting::run(MdogSimpleController* ctrl) {
     //设置所有cmd数据为0
+    std::vector<std::array<double, 3>> control_points = {
+        {0.30, 0.0, 0.15},      // 起点
+        {0.10, 0.0, 0.0},     // 抬腿中点
+        {0.30, 0.0, -0.15},     // 落点
+    };
+    // 起点和终点
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    // 时间推进
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    // 对角腿同步，交错运动，相位差0.5
     for (int leg = 0; leg < 4; ++leg) {
+        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
+        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
+        double phase = std::fmod(t + phase_offset, 1.0);
+
+        std::array<double, 3> foot_pos;
+        if (phase < 0.5) {
+            // 摆动相：贝塞尔曲线
+            double swing_t = phase / 0.5; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
+        } else {
+            // 支撑相：直线连接首尾
+            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
+            }
+        }
+
+        std::array<double, 3> theta;
+        std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            // 左右腿hip方向相反，FR(0) RR(2)为右腿，FL(1) RL(3)为左腿
+            if (leg == 1 || leg == 3) { // 右腿
+                theta[0] = -theta[0];
+            }
             for (int j = 0; j < 3; ++j) {
-            ctrl->data_.command[leg].torque_des[j] = 0;
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
-            ctrl->data_.command[leg].speed_des[j] = 0;
+                ctrl->data_.command_real[leg].speed_des[j] = 0;
-            ctrl->data_.command[leg].pos_des[j] = 0;
+                ctrl->data_.command_real[leg].torque_des[j] = 0;
-            ctrl->data_.command[leg].kp[j] = 0;
+                ctrl->data_.command_real[leg].kp[j] = 0.8;
-            ctrl->data_.command[leg].kd[j] = 0.05;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
             }
         }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
 }
 
 void StateTroting::exit(MdogSimpleController* ctrl) {

src/controllers/mdog_simple_controller/src/common/pid.cpp

@@ -0,0 +1,33 @@
+#include "mdog_simple_controller/common/pid.hpp"
+
+namespace mdog_simple_controller {
+
+PID::PID(double kp, double ki, double kd, double dt, double max_out, double min_out)
+    : kp_(kp), ki_(ki), kd_(kd), dt_(dt), max_out_(max_out), min_out_(min_out),
+      prev_error_(0.0), integral_(0.0) {}
+
+void PID::set_gains(double kp, double ki, double kd) {
+    kp_ = kp; ki_ = ki; kd_ = kd;
+}
+
+void PID::set_limits(double min_out, double max_out) {
+    min_out_ = min_out; max_out_ = max_out;
+}
+
+void PID::reset() {
+    prev_error_ = 0.0;
+    integral_ = 0.0;
+}
+
+double PID::compute(double setpoint, double measurement) {
+    double error = setpoint - measurement;
+    integral_ += error * dt_;
+    double derivative = (error - prev_error_) / dt_;
+    double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
+    if (output > max_out_) output = max_out_;
+    if (output < min_out_) output = min_out_;
+    prev_error_ = error;
+    return output;
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -77,9 +77,6 @@ void MdogSimpleController::control_loop() {
                 data_.feedback[leg].torque[j] = data_.command[leg].torque_des[j];
                 data_.feedback[leg].speed[j]  = data_.command[leg].speed_des[j];
                 data_.feedback[leg].pos[j]    = data_.command[leg].pos_des[j];
-                // data_.feedback_real[leg].torque[j] = data_.command[leg].torque_des[j];
-                // data_.feedback_real[leg].speed[j]  = data_.command[leg].speed_des[j];
-                // data_.feedback_real[leg].pos[j]    = data_.command[leg].pos_des[j];
             }
         }
         feedback_to_real(data_, data_);