diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml
index 08542302..9626b78a 100644
--- a/config/cabin_robot.xml
+++ b/config/cabin_robot.xml
@@ -105,7 +105,7 @@
-
+
diff --git a/src/devices/biohead/biohead_esp32/include/biohead_esp32.h b/src/devices/biohead/biohead_esp32/include/biohead_esp32.h
index 0e6937ae..52b34d5d 100644
--- a/src/devices/biohead/biohead_esp32/include/biohead_esp32.h
+++ b/src/devices/biohead/biohead_esp32/include/biohead_esp32.h
@@ -44,6 +44,10 @@ namespace cmvr::device {
uint16_t angleToRaw(double angle);
double normalizeToAngle(double normalized, size_t index);
+ void sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms);
+
+
+
std::shared_ptr serial_;
std::string port_name_;
diff --git a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
index 91d6ecf8..9064ed4b 100644
--- a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
+++ b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
@@ -290,95 +290,6 @@ void BioHeadRobot::speakstop() {
LOG(INFO) << "[BioHeadRobot] speak thread stopped.";
}
-// void BioHeadRobot::speakthread() {
-// LOG(INFO) << "[BioHeadRobot] speakthread running.";
-//
-// // 固定参数
-// const double freq = 3.0; // Hz
-// const int step_ms = 40; // 下发周期（ms）
-// const double closed_angle = 90.0;
-// const double open8 = 83.0; // ch8 张嘴角
-// const double open9 = 97.0; // ch9 张嘴角
-//
-// // 找到 channels_ 中对应索引（addr=65 ch=8/9）
-// int idx8 = -1, idx9 = -1;
-// for (size_t i = 0; i < channels_.size(); ++i) {
-// if (channels_[i].addr == 65 && channels_[i].channel == 8) idx8 = static_cast(i);
-// if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast(i);
-// }
-// if (idx8 < 0 || idx9 < 0) {
-// LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread.";
-// speak_running_.store(false);
-// return;
-// }
-//
-// // 以当前角度为基准
-// std::vector base = current_joints_;
-// if (base.size() != channels_.size()) base.resize(channels_.size(), 90.0);
-//
-// auto start = std::chrono::steady_clock::now();
-//
-// while (speak_running_.load()) {
-// auto now = std::chrono::steady_clock::now();
-// double t = std::chrono::duration_cast>(now - start).count();
-//
-// // 正弦产生 0..1 区间因子
-// double phase = 2.0 * M_PI * freq * t;
-// double s = 0.5 * (1.0 + std::sin(phase)); // 0..1
-//
-// // 计算角度（直接使用度数，不用normalizeToAngle）
-// double target8 = closed_angle + (open8 - closed_angle) * s;
-// double target9 = closed_angle + (open9 - closed_angle) * s;
-//
-// // clamp 到限位
-// target8 = std::clamp(target8, min_angles_[idx8], max_angles_[idx8]);
-// target9 = std::clamp(target9, min_angles_[idx9], max_angles_[idx9]);
-//
-// // 构建目标角数组（只修改 idx8/idx9）
-// std::vector tgt = base;
-// tgt[idx8] = target8;
-// tgt[idx9] = target9;
-//
-// // ---- 构造 ESP32 协议帧 ----
-// std::vector raw_data;
-// for (int idx : {idx8, idx9}) {
-// uint16_t angle_raw = static_cast(std::round(tgt[idx])); // 直接角度
-// raw_data.push_back(channels_[idx].addr); // 地址
-// raw_data.push_back(channels_[idx].channel); // 通道
-// raw_data.push_back(angle_raw); // 角度（1字节即可）
-// raw_data.push_back(step_ms & 0xFF); // 持续时间低字节
-// raw_data.push_back((step_ms >> 8) & 0xFF); // 持续时间高字节
-// }
-//
-// // 打印发送数据
-// std::string json_output = "{ \"raw_data\": [";
-// for (size_t i = 0; i < raw_data.size(); ++i) {
-// json_output += std::to_string(raw_data[i]);
-// if (i < raw_data.size() - 1) json_output += ", ";
-// }
-// json_output += "] }";
-// std::cout << "Sending data at step: " << json_output << std::endl;
-//
-// // 下发
-// serial_->sendRawServoData(raw_data);
-//
-// // 控制循环频率
-// std::this_thread::sleep_for(std::chrono::milliseconds(step_ms));
-// }
-//
-// // 退出前恢复基准角
-// std::vector restore_data;
-// for (int idx : {idx8, idx9}) {
-// uint16_t angle_raw = static_cast(std::round(base[idx]));
-// restore_data.push_back(channels_[idx].addr);
-// restore_data.push_back(channels_[idx].channel);
-// restore_data.push_back(angle_raw);
-// restore_data.push_back(200 & 0xFF);
-// restore_data.push_back((200 >> 8) & 0xFF);
-// }
-// serial_->sendRawServoData(restore_data);
-// LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
-// }
void BioHeadRobot::speakthread() {
LOG(INFO) << "[BioHeadRobot] speakthread running.";
@@ -576,20 +487,10 @@ void BioHeadRobot::speakthread() {
LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
}
-
-
-
-void BioHeadRobot::expressionHappy() {
- const int step_ms = 200;
-
- // 设备64角度（10通道）
- const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
- // 设备65角度（10通道）
- const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
-
+void BioHeadRobot::sendExpression(const std::vector& device_64_angles, const std::vector& device_65_angles, int step_ms) {
std::vector raw_data;
- // 下发设备64的10个通道（0~9）
+ // 处理设备64角度
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast(std::round(device_64_angles[ch]));
raw_data.push_back(64); // 地址
@@ -599,7 +500,7 @@ void BioHeadRobot::expressionHappy() {
raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
}
- // 下发设备65的10个通道（0~9）
+ // 处理设备65角度
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast(std::round(device_65_angles[ch]));
raw_data.push_back(65); // 地址
@@ -612,6 +513,7 @@ void BioHeadRobot::expressionHappy() {
serial_->sendRawServoData(raw_data);
std::this_thread::sleep_for(std::chrono::seconds(3));
+ // 恢复到原始角度
// 设备64角度（10通道）
const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
// 设备65角度（10通道）
@@ -642,289 +544,41 @@ void BioHeadRobot::expressionHappy() {
serial_->sendRawServoData(raw_data_neutral);
}
+void BioHeadRobot::expressionHappy() {
+ const std::vector device_64_angles = {60, 90, 120, 90, 35, 145, 160, 25, 90, 90};
+ const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
+ sendExpression(device_64_angles, device_65_angles, 0);
+}
void BioHeadRobot::expressionSurprised() {
- const int step_ms = 200;
-
- const std::vector device_64_angles = {60, 170, 120, 20, 35, 145, 160, 25, 90, 90};
- const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
-
- std::vector raw_data;
-
- for (size_t i = 0; i < 10; ++i) {
- int idx = -1;
- for (size_t j = 0; j < channels_.size(); ++j) {
- if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) {
- idx = j;
- break;
- }
- }
- if (idx >= 0) {
- uint8_t angle = static_cast(std::round(device_64_angles[i]));
- raw_data.push_back(64);
- raw_data.push_back(4 + i);
- raw_data.push_back(angle);
- raw_data.push_back(step_ms & 0xFF);
- raw_data.push_back((step_ms >> 8) & 0xFF);
- }
- }
-
- for (size_t i = 0; i < 10; ++i) {
- int idx = -1;
- for (size_t j = 0; j < channels_.size(); ++j) {
- if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) {
- idx = j;
- break;
- }
- }
- if (idx >= 0) {
- uint8_t angle = static_cast(std::round(device_65_angles[i]));
- raw_data.push_back(65);
- raw_data.push_back(8 + i);
- raw_data.push_back(angle);
- raw_data.push_back(step_ms & 0xFF);
- raw_data.push_back((step_ms >> 8) & 0xFF);
- }
- }
-
- serial_->sendRawServoData(raw_data);
+ const std::vector device_64_angles = {60, 90, 90, 90, 90, 90, 90, 90, 90, 90};
+ const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 85, 95};
+ sendExpression(device_64_angles, device_65_angles, 200);
}
void BioHeadRobot::expressionTired() {
- const int step_ms = 200;
-
const std::vector device_64_angles = {90, 90, 90, 90, 75, 105, 105, 75, 90, 90};
const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
-
- std::vector raw_data;
-
- for (size_t i = 0; i < 10; ++i) {
- int idx = -1;
- for (size_t j = 0; j < channels_.size(); ++j) {
- if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) {
- idx = j;
- break;
- }
- }
- if (idx >= 0) {
- uint8_t angle = static_cast(std::round(device_64_angles[i]));
- raw_data.push_back(64);
- raw_data.push_back(4 + i);
- raw_data.push_back(angle);
- raw_data.push_back(step_ms & 0xFF);
- raw_data.push_back((step_ms >> 8) & 0xFF);
- }
- }
-
- for (size_t i = 0; i < 10; ++i) {
- int idx = -1;
- for (size_t j = 0; j < channels_.size(); ++j) {
- if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) {
- idx = j;
- break;
- }
- }
- if (idx >= 0) {
- uint8_t angle = static_cast(std::round(device_65_angles[i]));
- raw_data.push_back(65);
- raw_data.push_back(8 + i);
- raw_data.push_back(angle);
- raw_data.push_back(step_ms & 0xFF);
- raw_data.push_back((step_ms >> 8) & 0xFF);
- }
- }
-
- serial_->sendRawServoData(raw_data);
+ sendExpression(device_64_angles, device_65_angles, 200);
}
void BioHeadRobot::expressionAngry() {
- const int step_ms = 200;
-
- // 设备64角度（10通道）
const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
- // 设备65角度（10通道）
const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
-
- std::vector raw_data;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_angles[ch]));
- raw_data.push_back(64); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_angles[ch]));
- raw_data.push_back(65); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data);
- std::this_thread::sleep_for(std::chrono::seconds(3));
-
- // 设备64角度（10通道）
- const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
- // 设备65角度（10通道）
- const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
-
- std::vector raw_data_neutral;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_neutral[ch]));
- raw_data_neutral.push_back(64); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_neutral[ch]));
- raw_data_neutral.push_back(65); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data_neutral);
+ sendExpression(device_64_angles, device_65_angles, 200);
}
void BioHeadRobot::expressionSadness() {
- const int step_ms = 200;
-
- // 设备64角度（10通道）
const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
- // 设备65角度（10通道）
const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
-
- std::vector raw_data;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_angles[ch]));
- raw_data.push_back(64); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_angles[ch]));
- raw_data.push_back(65); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data);
- std::this_thread::sleep_for(std::chrono::seconds(3));
-
- // 设备64角度（10通道）
- const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
- // 设备65角度（10通道）
- const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
-
- std::vector raw_data_neutral;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_neutral[ch]));
- raw_data_neutral.push_back(64); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_neutral[ch]));
- raw_data_neutral.push_back(65); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data_neutral);
+ sendExpression(device_64_angles, device_65_angles, 200);
}
void BioHeadRobot::expressionYawn() {
- const int step_ms = 200;
-
- // 设备64角度（10通道）
const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
- // 设备65角度（10通道）
const std::vector device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
-
- std::vector raw_data;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_angles[ch]));
- raw_data.push_back(64); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_angles[ch]));
- raw_data.push_back(65); // 地址
- raw_data.push_back(ch); // 通道号
- raw_data.push_back(angle); // 角度
- raw_data.push_back(step_ms & 0xFF); // 低字节
- raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data);
- std::this_thread::sleep_for(std::chrono::seconds(3));
-
- // 设备64角度（10通道）
- const std::vector device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
- // 设备65角度（10通道）
- const std::vector device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
-
- std::vector raw_data_neutral;
-
- // 下发设备64的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_64_neutral[ch]));
- raw_data_neutral.push_back(64); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- // 下发设备65的10个通道（0~9）
- for (int ch = 0; ch < 10; ++ch) {
- uint8_t angle = static_cast(std::round(device_65_neutral[ch]));
- raw_data_neutral.push_back(65); // 地址
- raw_data_neutral.push_back(ch); // 通道号
- raw_data_neutral.push_back(angle); // 角度
- raw_data_neutral.push_back(step_ms & 0xFF); // 低字节
- raw_data_neutral.push_back((step_ms >> 8) & 0xFF); // 高字节
- }
-
- serial_->sendRawServoData(raw_data_neutral);
+ sendExpression(device_64_angles, device_65_angles, 200);
}