70 lines
1.6 KiB
C++
70 lines
1.6 KiB
C++
/**
|
|
* @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_;}
|
|
};
|