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-18 11:40:08 +08:00
parent 058ea0f505
commit de13038895
2 changed files with 37 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -290,19 +290,20 @@ 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 = 75; // ch8 张嘴角
const double open9 = 105; // ch9 张嘴角
const double open8 = 75; // ch8 张嘴角 - 从75改为60
const double open9 = 105; // ch9 张嘴角 - 从105改为120
// 眼睛舵机参数 (addr=64, channels=4~8)
const std::vector<double> eye_open_angles = {40, 120, 125, 50, 90}; // 睁开角度
const std::vector<double> eye_open_angles = {40, 120, 110, 60, 90}; // 睁开角度
const std::vector<double> eye_close_angles = {90, 90, 90, 90, 90}; // 闭合角度
// 找到眼睛舵机索引 (addr=64, channels=4~8)
@ -311,7 +312,6 @@ void BioHeadRobot::speakthread() {
if (channels_[i].addr == 64 && channels_[i].channel >= 4 && channels_[i].channel <= 8) {
eye_indexes.push_back(static_cast<int>(i));
}
}
// 找到嘴角舵机索引（addr=65 ch=8/9）
@ -356,6 +356,11 @@ void BioHeadRobot::speakthread() {
// 眼睛状态（true=睁开，false=闭合）
bool eyes_open = true;
// 嘴角运动的随机扰动相关变量
double last_random_update = 0.0;
double current_random_factor = 0.0;
const double random_update_interval = 0.2; // 每0.2秒更新一次随机扰动
while (speak_running_.load()) {
auto now = std::chrono::steady_clock::now();
double t = std::chrono::duration_cast<std::chrono::duration<double>>(now - start).count();
@ -386,11 +391,21 @@ void BioHeadRobot::speakthread() {
}
}
// 正弦产生 0..1 区间因子（嘴角运动）
double phase = 2.0 * M_PI * freq * t;
double s = 0.5 * (1.0 + std::sin(phase)); // 0..1
// 定期更新嘴角的随机扰动因子
if (t - last_random_update > random_update_interval) {
current_random_factor = simple_rand(-0.2, 0.2);
last_random_update = t;
}
// 计算嘴角角度（直接使用度数，不用normalizeToAngle）
// 修改后的嘴角运动计算
double phase = 2.0 * M_PI * freq * t;
// 增加振幅并添加随机扰动
double base_s = 0.5 * (1.0 + 1.3 * std::sin(phase));
double s = base_s + current_random_factor;
s = std::clamp(s, 0.0, 1.0);
// 计算嘴角角度
double target8 = closed_angle + (open8 - closed_angle) * s;
double target9 = closed_angle + (open9 - closed_angle) * s;
@ -487,6 +502,7 @@ void BioHeadRobot::speakthread() {
LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
}
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;
@ -517,7 +533,7 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
// 设备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};
const std::vector<double> device_65_neutral = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
std::vector<uint8_t> raw_data_neutral;
@ -547,38 +563,38 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
//高兴
void BioHeadRobot::expressionHappy() {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 70, 135, 120, 50, 90, 90};
const std::vector<double> device_65_angles = {100, 80, 125, 135, 100, 80, 70, 80, 85, 95};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}
//惊讶
void BioHeadRobot::expressionSurprised() {
const std::vector<double> device_64_angles = {60, 90, 120, 90, 35, 160, 160, 25, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 80, 100};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}
//睡觉
void BioHeadRobot::expressionTired() {
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};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}
//愤怒
void BioHeadRobot::expressionAngry() {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 60, 120, 120, 60, 90, 90};
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}
//悲伤
void BioHeadRobot::expressionSadness() {
const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 115, 110, 70, 90, 90};
const std::vector<double> device_65_angles = {100, 80, 130, 55, 90, 55, 110, 85, 85, 95};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}
//打哈欠
void BioHeadRobot::expressionYawn() {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90};
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 110, 90, 90};
sendExpression(device_64_angles, device_65_angles, 0);
}

8
src/service/src/grpc_head_service.cpp
View File

@ -209,8 +209,6 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
auto robot1 = dmgr_.getDevice<AbstractRobot>("hc01");
std::vector<JointPoint> cmds{};
if (!robot) {
throw runtime_error("Biohead device not found");
@ -219,7 +217,6 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
robot->eStop(); // 停止执行
robot->emergency_stop_requested = true; // ✅ 设置中断标志
robot1->moveJ(cmds);
@ -240,15 +237,20 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c
{
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
std::vector<JointPoint> cmds{};
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->speakstart(); // kaish开始
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;