修正遥控器映射

build.sh

@@ -0,0 +1,2 @@
+source install/setup.bash
+colcon build

src/commands/joystick_input/src/JoystickInput.cpp

@@ -13,24 +13,22 @@ JoystickInput::JoystickInput() : Node("joysick_input_node") {
 }
 
 void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) {
-    if (msg->buttons[1] && msg->buttons[4]) {
-        inputs_.command = 1; // LB + B
-    } else if (msg->buttons[0] && msg->buttons[4]) {
-        inputs_.command = 2; // LB + A
-    } else if (msg->buttons[2] && msg->buttons[4]) {
-        inputs_.command = 3; // LB + X
-    } else if (msg->buttons[3] && msg->buttons[4]) {
-        inputs_.command = 4; // LB + Y
-    } else if (msg->axes[2] != 1 && msg->buttons[1]) {
-        inputs_.command = 5; // LT + B
-    } else if (msg->axes[2] != 1 && msg->buttons[0]) {
-        inputs_.command = 6; // LT + A
-    } else if (msg->axes[2] != 1 && msg->buttons[3]) {
-        inputs_.command = 7; // LT + X
-    } else if (msg->axes[2] != 1 && msg->buttons[4]) {
-        inputs_.command = 8; // LT + Y
+    if (msg->buttons[0]) {
+        inputs_.command = 1; // BACK
+    } else if (msg->buttons[1]) {
+        inputs_.command = 2; // RB
+    } else if (msg->buttons[3]) {
+        inputs_.command = 3; // LB
+    } else if (msg->buttons[4]) {
+        inputs_.command = 4; // Y
+    } else if (msg->buttons[6]) {
+        inputs_.command = 5; // X
     } else if (msg->buttons[7]) {
-        inputs_.command = 9; // START
+        inputs_.command = 6; // B
+    } else if (msg->buttons[8]) {
+        inputs_.command = 7; // A
+    } else if (msg->buttons[9]) {
+         inputs_.command = 8;
     } else {
         inputs_.command = 0;
         inputs_.lx = -msg->axes[0];

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -22,6 +22,7 @@ include_directories(
 add_executable(mdog_simple_controller
   src/mdog_simple_controller.cpp
   src/FSM/state_safe.cpp
+  src/FSM/state_stand.cpp
   src/FSM/state_zero.cpp
   src/FSM/state_walk.cpp
   src/FSM/state_balance.cpp

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp

@@ -0,0 +1,22 @@
+#pragma once
+#include <array>
+#include "mdog_simple_controller/FSM/fsm_state.hpp"
+
+namespace mdog_simple_controller {
+
+class StateStand : public FSMState {
+public:
+    void enter(MdogSimpleController*) override;
+    void run(MdogSimpleController*) override;
+    void exit(MdogSimpleController*) override;
+
+private:
+    bool inited = false;
+
+    std::array<std::array<double, 3>, 4> stand_pos;
+
+    std::array<std::array<double, 3>, 4> start_pos;
+
+};
+
+}

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

@@ -23,7 +23,12 @@ constexpr float JOINT_RATIO[4][3] = {
 // feedback → real
 void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst);
 
+// real → feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst);
+
 // real cmd → 输出 cmd
 void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
 
+
+
 } // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp

@@ -7,6 +7,7 @@
 #include "sensor_msgs/msg/joint_state.hpp"
 #include "geometry_msgs/msg/vector3.hpp"
 #include "mdog_simple_controller/FSM/state_safe.hpp"
+#include "mdog_simple_controller/FSM/state_stand.hpp"
 #include "mdog_simple_controller/FSM/state_zero.hpp"
 #include "mdog_simple_controller/FSM/state_walk.hpp"
 #include "mdog_simple_controller/FSM/state_balance.hpp"
@@ -75,6 +76,7 @@ private:
 
     FSMState* current_state_{nullptr};
     StateSafe state_safe_;
+    StateStand state_stand_;
     StateZero state_zero_;
     StateWalk state_walk_;
     StateBalance state_balance_;

src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp

@@ -0,0 +1,18 @@
+#include "mdog_simple_controller/FSM/state_stand.hpp"
+#include "mdog_simple_controller/mdog_simple_controller.hpp"
+
+namespace mdog_simple_controller {
+
+void StateStand::enter(MdogSimpleController* ctrl) {
+
+}
+
+void StateStand::run(MdogSimpleController* ctrl) {
+    // SAFE状态下的控制逻辑
+}
+
+void StateStand::exit(MdogSimpleController* ctrl) {
+    // 离开SAFE状态时的清理
+}
+
+}

src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp

@@ -40,24 +40,11 @@ void StateWalk::run(MdogSimpleController* ctrl) {
     double dt = (now - last_time).seconds();
     last_time = now;
 
-    static std::array<std::array<double, 3>, 4> start_theta; // 回位起点
-    static bool start_theta_set = false;
-
     if (!homing_done) {
-        if (!start_theta_set) {
-            // 只在回位开始时记录一次
-            for (int leg = 0; leg < 4; ++leg) {
-                for (int j = 0; j < 3; ++j) {
-                    start_theta[leg][j] = ctrl->data_.feedback_real[leg].pos[j];
-                }
-            }
-            start_theta_set = true;
-        }
-
         homing_time += dt;
         double alpha = std::min(homing_time / homing_duration, 1.0); // 插值系数
 
-        std::array<double, 3> home_pos = {0.0, 0.0, -0.4};
+        std::array<double, 3> home_pos = {0.0, 0.0, -0.35};
         const auto& link = kinematics::get_leg_link_length();
         const auto& hip_signs = kinematics::get_hip_signs();
 
@@ -67,9 +54,10 @@ void StateWalk::run(MdogSimpleController* ctrl) {
         for (int leg = 0; leg < 4; ++leg) {
             std::array<double, 3> theta;
             for (int j = 0; j < 3; ++j) {
-                // 插值到目标关节角度
-                double tgt = target_theta[j] * hip_signs[leg];
-                theta[j] = (1 - alpha) * start_theta[leg][j] + alpha * tgt;
+                // 每次都用当前位置做起点
+                double start = ctrl->data_.feedback_real[leg].pos[j];
+                double tgt = (j == 0) ? (target_theta[j] * hip_signs[leg]) : target_theta[j];
+                theta[j] = (1 - alpha) * start + alpha * tgt;
                 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;
@@ -81,16 +69,15 @@ void StateWalk::run(MdogSimpleController* ctrl) {
 
         if (homing_time >= homing_duration) {
             homing_done = true;
-            start_theta_set = false; // 下次回位可重新记录
         }
         return;
     }
 
-    // ...existing code...
+    // 以下内容必须在 run 函数体内
     std::vector<std::array<double, 3>> control_points = {
-        {0.15, 0.0, -0.40},    // 起点
-        {0.10, 0.0, -0.2},     // 抬腿中点
-        {-0.15, 0.0, -0.40},   // 落点
+        {0.25, 0.0, -0.35},      // 起点
+        {0.1, 0.0, -0.25},     // 抬腿中点
+        {-0.05, 0.0, -0.35},     // 落点
     };
     const auto& p0 = control_points.front();
     const auto& p1 = control_points.back();
@@ -133,8 +120,6 @@ void StateWalk::run(MdogSimpleController* ctrl) {
     realcmd_to_cmd(ctrl->data_, ctrl->data_);
 }
 
-// ...existing code...
-
 void StateWalk::exit(MdogSimpleController* ctrl) {
     // 可选：状态退出时清理
 }

src/controllers/mdog_simple_controller/src/common/user_math.cpp

@@ -16,6 +16,20 @@ void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst) {
     }
 }
 
+// real -> feedback
+void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst) {
+    for (int leg = 0; leg < 4; ++leg) {
+        for (int j = 0; j < 3; ++j) {
+            // 位置：乘以减速比，加offset
+            dst.feedback[leg].pos[j] = src.feedback_real[leg].pos[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j];
+            // 速度：乘以减速比
+            dst.feedback[leg].speed[j] = src.feedback_real[leg].speed[j] * JOINT_RATIO[leg][j];
+            // 力矩：除以减速比
+            dst.feedback[leg].torque[j] = src.feedback_real[leg].torque[j] / JOINT_RATIO[leg][j];
+        }
+    }
+}
+
 // real cmd → 输出 cmd
 void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
     for (int leg = 0; leg < 4; ++leg) {
@@ -33,4 +47,9 @@ void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
     }
 }
 
+// 插补函数
+double interpolate(double start, double end, double alpha) {
+    return (1 - alpha) * start + alpha * end;
+}
+
 } // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -48,7 +48,7 @@ void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs:
     input_cmd_.voltage.vx = msg->lx;
     input_cmd_.voltage.vy = msg->ly;
     input_cmd_.voltage.wz = msg->rx;
-    input_cmd_.hight = 0.35;
+    // input_cmd_.hight = 0.35;
     input_cmd_.status = msg->command;
 }
 
@@ -75,12 +75,18 @@ void MdogSimpleController::control_loop() {
     if (if_no_hardware_) {
         for (int leg = 0; leg < 4; ++leg) {
             for (int j = 0; j < 3; ++j) {
-                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[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_real[leg].torque_des[j];
+                data_.feedback_real[leg].speed[j]  = data_.command_real[leg].speed_des[j];
+                data_.feedback_real[leg].pos[j]    = data_.command_real[leg].pos_des[j];
+                data_.feedback[leg].torque[j] = data_.command_real[leg].torque_des[j];
+                data_.feedback[leg].speed[j]  = data_.command_real[leg].speed_des[j];
             }
         }
-        feedback_to_real(data_, data_);
+        // feedback_to_real(data_, data_);
+        // real_to_feedback(data_, data_);
     }
 
     rc_msgs::msg::LegCmd msg;