78 lines
2.1 KiB
C++
78 lines
2.1 KiB
C++
#ifndef CMVR_ES_BIOHEAD_ESP32_H
|
|
#define CMVR_ES_BIOHEAD_ESP32_H
|
|
|
|
#include "../../abstract_biohead.h"
|
|
#include "../../../../hardware/include/esp32_serial_port.h"
|
|
#include <vector>
|
|
#include <string>
|
|
#include <memory>
|
|
#include <mutex>
|
|
|
|
namespace cmvr::device {
|
|
|
|
struct ServoChannel {
|
|
uint8_t addr;
|
|
uint8_t channel;
|
|
};
|
|
|
|
class BioHeadRobot : public AbstractBiohead {
|
|
public:
|
|
explicit BioHeadRobot(const XmlNode &config);
|
|
~BioHeadRobot() override = default;
|
|
|
|
// 接口实现
|
|
void getState(RobotState &state) override;
|
|
void eStop() override;
|
|
void setExpressionPose(FacialExpressionState& expression_state, double vel = 0.5, double acc = 0.1) override;
|
|
void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override;
|
|
void speakstart() override;
|
|
void speakstop() override;
|
|
void speakthread();
|
|
|
|
void expressionHappy()override;
|
|
void expressionSurprised()override;
|
|
void expressionTired()override;
|
|
void expressionAngry() override;
|
|
void expressionSadness()override;
|
|
void expressionYawn()override;
|
|
|
|
|
|
private:
|
|
// 内部方法
|
|
void parseXmlConfig(const XmlNode &node);
|
|
void sendServoCommands( const std::vector<double>& targets, uint16_t duration_ms);
|
|
uint16_t angleToRaw(double angle);
|
|
double normalizeToAngle(double normalized, size_t index);
|
|
|
|
void sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms);
|
|
|
|
|
|
|
|
|
|
std::shared_ptr<SerialPort> serial_;
|
|
std::string port_name_;
|
|
|
|
std::vector<ServoChannel> channels_;
|
|
std::vector<double> current_joints_;
|
|
std::mutex stateMutex_; // 状态保护锁
|
|
|
|
std::vector<double> min_angles_;
|
|
std::vector<double> max_angles_;
|
|
|
|
|
|
double angle_deadband_{36};
|
|
std::vector<double> last_joints_; // 上次下发角度缓存
|
|
|
|
std::shared_ptr<std::thread> speak_thread_;
|
|
std::atomic<bool> speak_running_{false};
|
|
|
|
|
|
|
|
};
|
|
|
|
} // namespace cmvr::device
|
|
|
|
#endif // CMVR_ES_BIOHEAD_ESP32_H
|
|
|
|
|