From 5cee4645b3b504ae6b599142932d1d10081d6b94 Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Fri, 12 Dec 2025 09:31:35 +0800 Subject: [PATCH] update --- .../biohead_esp32/src/biohead_esp32.cpp | 43 ++++++++++--------- src/hardware/src/esp32_serial_port.cpp | 10 ++--- src/service/src/grpc_head_service.cpp | 10 ++++- 3 files changed, 36 insertions(+), 27 deletions(-) diff --git a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp index 9064ed4b..e17e59be 100644 --- a/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp +++ b/src/devices/biohead/biohead_esp32/src/biohead_esp32.cpp @@ -298,12 +298,12 @@ void BioHeadRobot::speakthread() { const double freq = 3.0; // Hz const int step_ms = 40; // 下发周期（ms） const double closed_angle = 90.0; - const double open8 = 80.0; // ch8 张嘴角 - const double open9 = 100.0; // ch9 张嘴角 + const double open8 = 75; // ch8 张嘴角 + const double open9 = 105; // ch9 张嘴角 // 眼睛舵机参数 (addr=64, channels=4~8) - const std::vector eye_open_angles = {40, 120, 125, 50, 75}; // 睁开角度 - const std::vector eye_close_angles = {90, 90, 90, 90, 75}; // 闭合角度 + const std::vector eye_open_angles = {40, 120, 125, 50, 90}; // 睁开角度 + const std::vector eye_close_angles = {90, 90, 90, 90, 90}; // 闭合角度 // 找到眼睛舵机索引 (addr=64, channels=4~8) std::vector eye_indexes; @@ -544,41 +544,42 @@ void BioHeadRobot::sendExpression(const std::vector& device_64_angles, c 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}; + const std::vector device_64_angles = {90, 90, 90, 90, 70, 135, 120, 50, 90, 90}; + const std::vector device_65_angles = {100, 80, 125, 135, 100, 80, 70, 80, 85, 95}; sendExpression(device_64_angles, device_65_angles, 0); } - +//惊讶 void BioHeadRobot::expressionSurprised() { - 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); + const std::vector device_64_angles = {60, 90, 120, 90, 35, 160, 160, 25, 90, 90}; + const std::vector device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 80, 100}; + sendExpression(device_64_angles, device_65_angles, 0); } - +//睡觉 void BioHeadRobot::expressionTired() { 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}; - sendExpression(device_64_angles, device_65_angles, 200); + sendExpression(device_64_angles, device_65_angles, 0); } - +//愤怒 void BioHeadRobot::expressionAngry() { - const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; + const std::vector device_64_angles = {90, 90, 90, 90, 60, 120, 120, 60, 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, 200); + sendExpression(device_64_angles, device_65_angles, 0); } - +//悲伤 void BioHeadRobot::expressionSadness() { - const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 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, 200); + const std::vector device_64_angles = {90, 70, 90, 110, 70, 115, 110, 70, 90, 90}; + const std::vector device_65_angles = {100, 80, 130, 55, 90, 55, 110, 85, 85, 95}; + sendExpression(device_64_angles, device_65_angles, 0); } - +//打哈欠 void BioHeadRobot::expressionYawn() { const std::vector device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 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, 200); + sendExpression(device_64_angles, device_65_angles, 0); } diff --git a/src/hardware/src/esp32_serial_port.cpp b/src/hardware/src/esp32_serial_port.cpp index 3d969d2e..2fc4fc1d 100644 --- a/src/hardware/src/esp32_serial_port.cpp +++ b/src/hardware/src/esp32_serial_port.cpp @@ -149,11 +149,11 @@ bool SerialPort::send(const std::vector& data) { } void SerialPort::printPacket(const std::vector& packet) { - std::cout << "发送数据包: "; - for (uint8_t byte : packet) { - std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; - } - std::cout << std::dec << std::endl; + // std::cout << "发送数据包: "; + // for (uint8_t byte : packet) { + // std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; + // } + // std::cout << std::dec << std::endl; } } // namespace cmvr diff --git a/src/service/src/grpc_head_service.cpp b/src/service/src/grpc_head_service.cpp index c5694f61..5c5e7638 100644 --- a/src/service/src/grpc_head_service.cpp +++ b/src/service/src/grpc_head_service.cpp @@ -209,6 +209,9 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( try { string dev_id = request->header().device_id(); auto robot = dmgr_.getDevice(dev_id); + auto robot1 = dmgr_.getDevice("hc01"); + std::vector cmds{}; + if (!robot) { throw runtime_error("Biohead device not found"); } @@ -216,6 +219,11 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop( robot->eStop(); // 停止执行 robot->emergency_stop_requested = true; // ✅ 设置中断标志 + robot1->moveJ(cmds); + + + + response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); return grpc::Status::OK; @@ -387,7 +395,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con throw runtime_error("Biohead device not found"); } - robot->expressionAngry(); // 停止执行 + robot->expressionSadness(); // 停止执行 response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp());