SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
tankaitao 2025-12-09 16:53:38 +08:00
parent f7003f0993
commit aa7da848b2
3 changed files with 21 additions and 363 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
config/cabin_robot.xml
View File

@ -105,7 +105,7 @@
</Microphone>
<Speaker>
<ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="100"/>
<ffmpegSpeaker id="spk1" serial="" alas="default" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="100"/>
</Speaker>
<Canbus>

4
src/devices/biohead/biohead_esp32/include/biohead_esp32.h
View File

@ -44,6 +44,10 @@ namespace cmvr::device {
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_;

378
src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp
View File

@ -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<int>(i);
// if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast<int>(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<double> 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<std::chrono::duration<double>>(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<double> tgt = base;
// tgt[idx8] = target8;
// tgt[idx9] = target9;
//
// // ---- 构造 ESP32 协议帧 ----
// std::vector<uint8_t> raw_data;
// for (int idx : {idx8, idx9}) {
// uint16_t angle_raw = static_cast<uint16_t>(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<uint8_t> restore_data;
// for (int idx : {idx8, idx9}) {
// uint16_t angle_raw = static_cast<uint16_t>(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<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, const std::vector<double>& device_65_angles, int step_ms) {
std::vector<uint8_t> raw_data;
// 下发设备64的10个通道（0~9）
// 处理设备64角度
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> 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<double> device_64_angles = {60, 90, 120, 90, 35, 145, 160, 25, 90, 90};
const std::vector<double> 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<double> device_64_angles = {60, 170, 120, 20, 35, 145, 160, 25, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
std::vector<uint8_t> 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<uint8_t>(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<uint8_t>(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<double> device_64_angles = {60, 90, 90, 90, 90, 90, 90, 90, 90, 90};
const std::vector<double> 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<double> device_64_angles = {90, 90, 90, 90, 75, 105, 105, 75, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
std::vector<uint8_t> 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<uint8_t>(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<uint8_t>(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<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
std::vector<uint8_t> raw_data;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
std::vector<uint8_t> raw_data_neutral;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
std::vector<uint8_t> raw_data;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
std::vector<uint8_t> raw_data_neutral;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
std::vector<uint8_t> raw_data;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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<double> device_64_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
std::vector<uint8_t> raw_data_neutral;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(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<uint8_t>(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);
}