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-12 09:31:35 +08:00
parent aa7da848b2
commit 5cee4645b3
3 changed files with 36 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -298,12 +298,12 @@ void BioHeadRobot::speakthread() {
const double freq = 3.0; // Hz const double freq = 3.0; // Hz
const int step_ms = 40; // 下发周期（ms） const int step_ms = 40; // 下发周期（ms）
const double closed_angle = 90.0; const double closed_angle = 90.0;
const double open8 = 80.0; // ch8 张嘴角 const double open8 = 75; // ch8 张嘴角
const double open9 = 100.0; // ch9 张嘴角 const double open9 = 105; // ch9 张嘴角
// 眼睛舵机参数 (addr=64, channels=4~8) // 眼睛舵机参数 (addr=64, channels=4~8)
const std::vector<double> eye_open_angles = {40, 120, 125, 50, 75}; // 睁开角度 const std::vector<double> eye_open_angles = {40, 120, 125, 50, 90}; // 睁开角度
const std::vector<double> eye_close_angles = {90, 90, 90, 90, 75}; // 闭合角度 const std::vector<double> eye_close_angles = {90, 90, 90, 90, 90}; // 闭合角度
// 找到眼睛舵机索引 (addr=64, channels=4~8) // 找到眼睛舵机索引 (addr=64, channels=4~8)
std::vector<int> eye_indexes; std::vector<int> eye_indexes;
@ -544,41 +544,42 @@ void BioHeadRobot::sendExpression(const std::vector<double>& device_64_angles, c
serial_->sendRawServoData(raw_data_neutral); serial_->sendRawServoData(raw_data_neutral);
} }
//高兴
void BioHeadRobot::expressionHappy() { 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_64_angles = {90, 90, 90, 90, 70, 135, 120, 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 = {100, 80, 125, 135, 100, 80, 70, 80, 85, 95};
sendExpression(device_64_angles, device_65_angles, 0); sendExpression(device_64_angles, device_65_angles, 0);
} }
//惊讶
void BioHeadRobot::expressionSurprised() { void BioHeadRobot::expressionSurprised() {
const std::vector<double> device_64_angles = {60, 90, 90, 90, 90, 90, 90, 90, 90, 90}; 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, 85, 95}; const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 80, 100};
sendExpression(device_64_angles, device_65_angles, 200); sendExpression(device_64_angles, device_65_angles, 0);
} }
//睡觉
void BioHeadRobot::expressionTired() { 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_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, 90, 83, 97};
sendExpression(device_64_angles, device_65_angles, 200); sendExpression(device_64_angles, device_65_angles, 0);
} }
//愤怒
void BioHeadRobot::expressionAngry() { void BioHeadRobot::expressionAngry() {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; 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, 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() { void BioHeadRobot::expressionSadness() {
const std::vector<double> device_64_angles = {90, 90, 90, 90, 40, 120, 125, 50, 90, 90}; const std::vector<double> device_64_angles = {90, 70, 90, 110, 70, 115, 110, 70, 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 = {100, 80, 130, 55, 90, 55, 110, 85, 85, 95};
sendExpression(device_64_angles, device_65_angles, 200); sendExpression(device_64_angles, device_65_angles, 0);
} }
//打哈欠
void BioHeadRobot::expressionYawn() { 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_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, 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);
} }

10
src/hardware/src/esp32_serial_port.cpp
View File

@ -149,11 +149,11 @@ bool SerialPort::send(const std::vector<uint8_t>& data) {
} }
void SerialPort::printPacket(const std::vector<uint8_t>& packet) { void SerialPort::printPacket(const std::vector<uint8_t>& packet) {
std::cout << "发送数据包: "; // std::cout << "发送数据包: ";
for (uint8_t byte : packet) { // for (uint8_t byte : packet) {
std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " "; // std::cout << "0x" << std::hex << std::setw(2) << std::setfill('0') << (int)byte << " ";
} // }
std::cout << std::dec << std::endl; // std::cout << std::dec << std::endl;
} }
} // namespace cmvr } // namespace cmvr

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

@ -209,6 +209,9 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
try { try {
string dev_id = request->header().device_id(); string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id); auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
auto robot1 = dmgr_.getDevice<AbstractRobot>("hc01");
std::vector<JointPoint> cmds{};
if (!robot) { if (!robot) {
throw runtime_error("Biohead device not found"); throw runtime_error("Biohead device not found");
} }
@ -216,6 +219,11 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
robot->eStop(); // 停止执行 robot->eStop(); // 停止执行
robot->emergency_stop_requested = true; // ✅ 设置中断标志 robot->emergency_stop_requested = true; // ✅ 设置中断标志
robot1->moveJ(cmds);
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK; return grpc::Status::OK;
@ -387,7 +395,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con
throw runtime_error("Biohead device not found"); throw runtime_error("Biohead device not found");
} }
robot->expressionAngry(); // 停止执行 robot->expressionSadness(); // 停止执行
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); setCurrentTimestamp(response->mutable_header()->mutable_timestamp());