initial commit

2026-10-22 09:23:54 +08:00
commit 63f557a39f
4448 changed files with 584487 additions and 0 deletions
@@ -0,0 +1,134 @@
+/**
+ * @file quadruped_state_machine.hpp
+ * @brief for robot to switch control state by user command input
+ * @author DeepRobotics
+ * @version 1.0
+ * @date 2025-11-07
+ * 
+ * @copyright Copyright (c) 2025  DeepRobotics
+ * 
+ */
+#pragma once
+
+#include "state_base.h"
+#include "state_machine_base.h"
+#include "quadruped_wheel/idle_state.hpp"
+#include "quadruped_wheel/standup_state.hpp"
+#include "quadruped_wheel/joint_damping_state.hpp"
+#include "quadruped_wheel/rl_control_state.hpp"
+#include "quadruped_wheel/liedown_state.hpp"
+#include "keyboard_interface.hpp"
+#include "hardware/m20_interface.hpp"
+#include "hardware/fire_ant_interface.hpp"
+#include "udp_server.hpp"
+
+namespace qw{
+class QwStateMachine : public StateMachineBase{
+private:
+
+    std::shared_ptr<StateBase> idle_controller_;
+    std::shared_ptr<StateBase> standup_controller_;
+    std::shared_ptr<StateBase> rl_controller_;
+    std::shared_ptr<StateBase> joint_damping_controller_;
+    std::shared_ptr<StateBase> car_move_controller_;
+    std::shared_ptr<StateBase> liedown_controller_;
+
+    std::shared_ptr<UdpServer> udp_server_;
+
+public:
+    const RobotName robot_name_;
+    const RemoteCommandType remote_cmd_type_;
+
+    QwStateMachine(RobotName robot_name, RemoteCommandType rct):StateMachineBase(RobotType::QuadrupedWheel),
+    robot_name_(robot_name),
+    remote_cmd_type_(rct) {}
+    ~QwStateMachine(){}
+
+    void Start(){
+        if(remote_cmd_type_ == RemoteCommandType::kKeyBoard){
+            uc_ptr_ = std::make_shared<KeyboardInterface>(robot_name_);
+        }else if(remote_cmd_type_ == RemoteCommandType::kGamepad){
+            auto gp_ptr = std::make_shared<GamepadInterface>(robot_name_);
+            udp_server_ = std::make_shared<UdpServer>(gp_ptr.get());
+            uc_ptr_ = gp_ptr;
+        }else{
+            std::cerr << "error user command interface! " << std::endl;
+            exit(0);
+        }
+        uc_ptr_->SetMotionStateFeedback(&StateBase::msfb_);
+
+        if(robot_name_ == RobotName::M20){
+   
+            ri_ptr_ = std::make_shared<M20Interface>("M20");
+            
+            cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
+        } else if (robot_name_ == RobotName::FireAnt) {
+            ri_ptr_ = std::make_shared<FireAntInterface>("FireAnt");
+            cp_ptr_ = std::make_shared<ControlParameters>(robot_name_);
+        } else {
+            throw std::runtime_error("Unsupported robot type");
+        }
+
+        std::shared_ptr<ControllerData> data_ptr = std::make_shared<ControllerData>();
+        data_ptr->ri_ptr = ri_ptr_;
+        data_ptr->uc_ptr = uc_ptr_;
+        data_ptr->cp_ptr = cp_ptr_;
+
+        sc_ptr_ = std::make_shared<SafeController>(QuadrupedWheel, "");
+        sc_ptr_->SetRobotDataSource(ri_ptr_);
+        sc_ptr_->SetUserCommandDataSource(uc_ptr_);
+
+        idle_controller_ = std::make_shared<IdleState>(robot_name_, "idle_state", data_ptr);
+        standup_controller_ = std::make_shared<StandUpState>(robot_name_, "standup_state", data_ptr);
+        rl_controller_ = std::make_shared<RLControlState>(robot_name_, "rl_control", data_ptr);
+        joint_damping_controller_ = std::make_shared<JointDampingState>(robot_name_, "joint_damping", data_ptr);
+        liedown_controller_ = std::make_shared<LieDownState>(robot_name_, "liedown_state", data_ptr);
+
+        current_controller_ = idle_controller_;
+        current_state_name_ = kIdle;
+        next_state_name_ = kIdle;
+
+        ri_ptr_->Start();
+        uc_ptr_->Start();
+        sc_ptr_->Start();
+        current_controller_->OnEnter();
+    }
+
+
+    std::shared_ptr<StateBase> GetStateControllerPtr(StateName state_name){
+        switch(state_name){
+            case StateName::kInvalid:{
+                return nullptr;
+            }
+            case StateName::kIdle:{
+                return idle_controller_;
+            }
+            case StateName::kStandUp:{
+                return standup_controller_;
+            }
+            case StateName::kRLControl:{
+                return rl_controller_;
+            }
+            case StateName::kJointDamping:{
+                return joint_damping_controller_;
+            }
+            case StateName::kLieDown:
+            {
+                return liedown_controller_;
+            }
+            default:{
+                std::cerr << "error state name" << std::endl;
+                return joint_damping_controller_;
+            }
+        }
+        return nullptr;
+    }
+
+    void Stop(){
+        sc_ptr_->Stop();
+        uc_ptr_->Stop();
+        ri_ptr_->Stop();
+    }
+
+};
+};