feat: 初始化 Go2 导航相关代码与配置

unitree_sdk2/example/state_machine/state_machine.hpp

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <algorithm>
+
+namespace unitree::common
+{
+    enum class STATES
+    {
+        DAMPING = 0,
+        STAND = 1,
+        CTRL = 2
+    };
+
+    class SimpleStateMachine
+    {
+    public:
+        SimpleStateMachine(double pd_ratio_init = 0.1, double delta_pd = 0.005) : state(STATES::STAND), pd_ratio(pd_ratio_init), delta_pd(delta_pd) {}
+
+        bool Stop()
+        {
+            state = STATES::DAMPING;
+            pd_ratio = 0.0;
+            return true;
+        }
+
+        bool Stand()
+        {
+            if (state == STATES::DAMPING || state == STATES::CTRL)
+            {
+                state = STATES::STAND;
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        bool Ctrl()
+        {
+            if (state == STATES::STAND && pd_ratio > 0.95)
+            {
+                state = STATES::CTRL;
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        void Standing(bool up = true)
+        {
+            if (state == STATES::STAND)
+            {
+                if (up)
+                {
+                    pd_ratio += delta_pd;
+                }
+                else
+                {
+                    pd_ratio -= delta_pd;
+                }
+                pd_ratio = std::max(0.0, std::min(1.0, pd_ratio));
+            }
+        }
+
+        STATES state;
+        double pd_ratio;
+        double delta_pd;
+    };
+} // namespace unitree