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 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<double> eye_open_angles = {40, 120, 125, 50, 75}; // 睁开角度
const std::vector<double> eye_close_angles = {90, 90, 90, 90, 75}; // 闭合角度
const std::vector<double> eye_open_angles = {40, 120, 125, 50, 90}; // 睁开角度
const std::vector<double> eye_close_angles = {90, 90, 90, 90, 90}; // 闭合角度
// 找到眼睛舵机索引 (addr=64, channels=4~8)
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);
}
//高兴
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};
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};
sendExpression(device_64_angles, device_65_angles, 0);
}
//惊讶
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_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 85, 95};
sendExpression(device_64_angles, device_65_angles, 200);
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};
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};
sendExpression(device_64_angles, device_65_angles, 200);
sendExpression(device_64_angles, device_65_angles, 0);
}
//愤怒
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};
sendExpression(device_64_angles, device_65_angles, 200);
sendExpression(device_64_angles, device_65_angles, 0);
}
//悲伤
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_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
sendExpression(device_64_angles, device_65_angles, 200);
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};
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};
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) {
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

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

@ -209,6 +209,9 @@ 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");
}
@ -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());