initial commit

2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
@@ -0,0 +1,69 @@
+/**
+ * @file motion_state_feedback.hpp
+ * @brief motion state feedback 
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+#include "common_types.h"
+
+class MotionStateFeedback
+{
+private:
+    uint8_t current_state_;
+    uint8_t current_gait_;
+    uint8_t last_state_;
+    uint8_t last_gait_;
+
+    Vec3f current_vel_;
+    Vec3f cmd_vel_;
+    Vec3f max_vel_;
+
+    union{
+        struct{
+            uint32_t joint_pos_limit : 1;
+            uint32_t posture_limit : 1;
+            uint32_t reserved : 30;
+        }error_code_bit_;
+        uint32_t error_code_;
+    };
+public:
+    MotionStateFeedback(/* args */){
+        memset(this, 0, sizeof(MotionStateFeedback));
+        std::cout << "Motion State Feedback" << std::endl;
+    }
+    ~MotionStateFeedback(){
+
+    }
+
+    void UpdateCurrentState(int state){
+        if(uint8_t(state) != current_state_){
+            last_state_ = current_state_;
+            current_state_ = uint8_t(state);
+        }
+    }
+
+    void UpdateCurrentGait(uint8_t gait){
+        if(gait != current_gait_){
+            last_gait_ = current_gait_;
+            current_gait_ = gait;
+        }
+    }
+
+    void ClearMotionError(){error_code_ = 0;}
+    void SetJointPosLimitError(){
+        error_code_bit_.joint_pos_limit = 1;
+    }
+    void SetPostureLimitError(){
+        error_code_bit_.posture_limit = 1;
+    }
+
+    inline uint8_t GetCurrentState(){return current_state_;}
+    inline uint8_t GetCurrentGait(){return current_gait_;}
+    inline uint8_t GetLastState(){return last_state_;}
+    inline uint8_t GetLastGait(){return last_gait_;}
+};