update robot controller
This commit is contained in:
parent
0720ec8171
commit
505b373368
@ -16,9 +16,9 @@ namespace cmvr::hardware
|
|||||||
cfg_ = cfg;
|
cfg_ = cfg;
|
||||||
id_ = cfg.getAttrString("id");
|
id_ = cfg.getAttrString("id");
|
||||||
joint_name_ = cfg.getAttrString("joint_name");
|
joint_name_ = cfg.getAttrString("joint_name");
|
||||||
limitQLb_ = cfg.getAttrDefault("limitQLb_",(float)3.14);
|
limitQLb_ = cfg.getAttrDefault("limitQLb_",3.14f);
|
||||||
limitQUb_ = cfg.getAttrDefault("limitQUb_",(float)3.14);
|
limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
|
||||||
limitQd = cfg.getAttrDefault("limitQd",(float)3.0);
|
limitQd = cfg.getAttrDefault("limitQd",3.0f);
|
||||||
}
|
}
|
||||||
~AbstractMotorProtocol() = default;
|
~AbstractMotorProtocol() = default;
|
||||||
|
|
||||||
|
|||||||
@ -1,4 +1,2 @@
|
|||||||
#add_subdirectory(ti5_robot)
|
|
||||||
add_subdirectory(humanoid_robot)
|
add_subdirectory(humanoid_robot)
|
||||||
#add_subdirectory(c701)
|
|
||||||
add_subdirectory(controller)
|
add_subdirectory(controller)
|
||||||
@ -1,54 +0,0 @@
|
|||||||
find_package(glog REQUIRED)
|
|
||||||
find_package(protobuf REQUIRED)
|
|
||||||
|
|
||||||
|
|
||||||
add_library(c701 SHARED
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_sdo_response.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo1.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo2.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_rpdo1.cpp)
|
|
||||||
|
|
||||||
|
|
||||||
target_include_directories(c701 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
||||||
|
|
||||||
add_library(cmvr_es::robot::c701 ALIAS c701)
|
|
||||||
|
|
||||||
target_link_libraries(c701
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::device::canbus
|
|
||||||
protobuf::libprotobuf
|
|
||||||
glog::glog
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
# --------------------------------------------------------
|
|
||||||
# Unit test
|
|
||||||
# --------------------------------------------------------
|
|
||||||
include_directories(
|
|
||||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
|
||||||
)
|
|
||||||
|
|
||||||
link_directories(
|
|
||||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(motor_controller_test
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller_test.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
target_link_libraries(motor_controller_test
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::device::canbus
|
|
||||||
cmvr_es::robot::c701
|
|
||||||
c701
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
glog::glog
|
|
||||||
proto-objects
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,400 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/17.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "robot/c701/motor/motor_controller.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_sdo_response.h"
|
|
||||||
#include "canbus/canopen/register.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_tpdo1.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_tpdo2.h"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
using namespace cmvr::robot::c701;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool enable_log) {
|
|
||||||
can_client->init();
|
|
||||||
can_client->start();
|
|
||||||
|
|
||||||
|
|
||||||
auto ret = ErrorCode::OK;
|
|
||||||
// 初始化 message_manager_
|
|
||||||
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
|
|
||||||
|
|
||||||
// NMT
|
|
||||||
message_manager_->AddSendProtocolData<NmtRequestProtocol<RobotDetail>, false>();
|
|
||||||
|
|
||||||
//sync
|
|
||||||
message_manager_->AddSendProtocolData<SyncProtocol<RobotDetail>, false>();
|
|
||||||
for (const auto node_id: node_ids_) {
|
|
||||||
//nmt
|
|
||||||
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
|
|
||||||
|
|
||||||
//sdo
|
|
||||||
message_manager_->AddSendProtocolData<SdoRequestProtocol<RobotDetail>, false>(node_id);
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorSdoResponse, false>(node_id);
|
|
||||||
|
|
||||||
//TPDO
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorTPDO1,false>(node_id);
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorTPDO2,false>(node_id);
|
|
||||||
|
|
||||||
//RPDO
|
|
||||||
message_manager_->AddSendProtocolData<Ti5MotorRPDO1,false>(node_id);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 初始化 sender
|
|
||||||
can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
|
|
||||||
ret = can_sender_->Init(can_client, enable_log);
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
LOG(ERROR) << "Failed to init can sender.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// 初始化 receiver
|
|
||||||
can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
|
||||||
ret = can_receiver_->Init(can_client, message_manager_.get(), enable_log);
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
LOG(ERROR) << "Failed to init can receiver.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// nmt
|
|
||||||
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
|
|
||||||
|
|
||||||
|
|
||||||
if (nmt_command_ == nullptr) {
|
|
||||||
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
|
|
||||||
|
|
||||||
// sync
|
|
||||||
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
|
|
||||||
|
|
||||||
|
|
||||||
if (sync_command_ == nullptr) {
|
|
||||||
LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
// can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
|
|
||||||
|
|
||||||
|
|
||||||
// sdo
|
|
||||||
for (auto node_id: node_ids_) {
|
|
||||||
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
|
|
||||||
|
|
||||||
if (sdo_commands_[node_id] == nullptr) {
|
|
||||||
LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
|
|
||||||
}
|
|
||||||
|
|
||||||
// pdo1
|
|
||||||
for (auto node_id: node_ids_) {
|
|
||||||
rpdo1_commands_[node_id] = dynamic_cast<Ti5MotorRPDO1 *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(Ti5MotorRPDO1::ID(node_id)));
|
|
||||||
|
|
||||||
if (rpdo1_commands_[node_id] == nullptr) {
|
|
||||||
LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// need sleep to ensure all messages received
|
|
||||||
LOG(INFO) << "Motor Controller is initialized.";
|
|
||||||
|
|
||||||
|
|
||||||
return ErrorCode::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ErrorCode MotorController::Start() {
|
|
||||||
auto ret = ErrorCode::OK;
|
|
||||||
ret = can_sender_->Start();
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
LOG(ERROR) << "Failed to start can sender.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
ret = can_receiver_->Start();
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
LOG(ERROR) << "Failed to start can receiver.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SetMode(uint8_t node_id, msgs::RunMode mode) {
|
|
||||||
|
|
||||||
cur_mode_ = mode;
|
|
||||||
// 1 : 先设置模式
|
|
||||||
auto data = static_cast<uint32_t>(mode);
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,OPERATION_MODE_6060,SUB_INDEX_0,data);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 2 : 状态机步进 —— Shutdown（0x06）
|
|
||||||
controlword_t cw = {};
|
|
||||||
cw.quick_stop = 1;
|
|
||||||
cw.enable_voltage = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
|
|
||||||
|
|
||||||
|
|
||||||
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
|
|
||||||
cw.switch_on = 1;
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
switch (mode) {
|
|
||||||
case RUN_MODE_PROFILE_POSITION: {
|
|
||||||
|
|
||||||
// 4 : 设置目标位置（为当前位置）
|
|
||||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
|
|
||||||
|
|
||||||
|
|
||||||
// 5 : 触发位置运动（new_set_point 翻转）
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
|
|
||||||
// 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case RUN_MODE_CYCLIC_SYNC_POSITION: {
|
|
||||||
// 设置目标位置为当前位置
|
|
||||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
|
|
||||||
|
|
||||||
//3 : 使能 15
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
cw.switch_on = 1;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CONTROL_WORD_6040,SUB_INDEX_0,cw.value);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
default:
|
|
||||||
// TODO: Handle unspecified or unknown mode
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::ConfigTPDO1(uint8_t node_id) {
|
|
||||||
//TDPO1 配置 状态字 和 控制字
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为异步
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_COMM_1800,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
|
|
||||||
|
|
||||||
// 3：配置约束时间 unit:0.1ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_3,1);
|
|
||||||
|
|
||||||
// 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_5,1000);
|
|
||||||
|
|
||||||
// 5 ：映射控制字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_1,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//6 : 映射状态字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_2,STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//7 : 映射模式
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_3,MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
|
|
||||||
|
|
||||||
//8 映射错误码
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_4,ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//9 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,4);
|
|
||||||
|
|
||||||
//10 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::ConfigTPDO2(uint8_t node_id) {
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为异步
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_COMM_1801,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
|
|
||||||
|
|
||||||
// 3：配置约束时间 unit:0.1ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_3,10000);
|
|
||||||
|
|
||||||
// 4 : 配置周期发送时间 unit : ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_5,1000);
|
|
||||||
|
|
||||||
// 5 ：映射当前位置
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_1,ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//6 : 映射当前速度
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_2,ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//9 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,2);
|
|
||||||
|
|
||||||
//10 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::ConfigRPDO1(uint8_t node_id,bool start) {
|
|
||||||
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_COMM_1400,SUB_INDEX_2,SYNC_EVENT_DRIVEN);
|
|
||||||
|
|
||||||
// // 3：配置约束时间 unit:0.1ms
|
|
||||||
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
|
|
||||||
//
|
|
||||||
// // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
|
|
||||||
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
|
|
||||||
|
|
||||||
// 5 ：映射位置
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_1,TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//6 : 映射控制字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_2,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
|
|
||||||
if (start) {
|
|
||||||
//7 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,2);
|
|
||||||
|
|
||||||
//8 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
|
|
||||||
|
|
||||||
controlword_t cw = {};
|
|
||||||
cw.switch_on = 1;
|
|
||||||
cw.enable_voltage = 1;
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
cw.quick_stop = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
|
|
||||||
// 1. 设置目标位置
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
|
|
||||||
|
|
||||||
|
|
||||||
// 2. 设置触发位（bit4 = 1）
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
|
|
||||||
|
|
||||||
// 3. 清除触发位（bit4 = 0），准备下一次触发
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
|
||||||
|
|
||||||
// 触发目标位置运动
|
|
||||||
controlword_t cw;
|
|
||||||
cw.value = 0x0F;
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
|
|
||||||
rpdo1_commands_[node_id]->SetTargetPos(pos);
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
||||||
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SetTargetPosition(uint8_t node_id, double angle_rad) {
|
|
||||||
|
|
||||||
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
|
||||||
|
|
||||||
switch (cur_mode_) {
|
|
||||||
case RUN_MODE_CYCLIC_SYNC_POSITION:
|
|
||||||
SetCSPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
break;
|
|
||||||
case RUN_MODE_PROFILE_POSITION:
|
|
||||||
SetPPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
// SetPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
break;
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
|
|
||||||
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
|
||||||
rpdo1_commands_[node_id]->SetTargetPos(pos);
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SeedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
|
|
||||||
nmt_command_->RequestService(node_id,command);
|
|
||||||
can_sender_->Update(nmt_command_->ID);
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SeedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms) {
|
|
||||||
sdo_commands_[node_id]->SetFrameData(cs,index,sub_index,data);
|
|
||||||
can_sender_->Update(sdo_commands_[node_id]->ID());
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,101 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/17.
|
|
||||||
//
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <utility>
|
|
||||||
|
|
||||||
#include "canbus/can_client/socket/socket_can_client_raw.h"
|
|
||||||
#include "can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/can_receiver.h"
|
|
||||||
#include "canbus/can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/message_manager.h"
|
|
||||||
#include "cmvr/msgs/error_code.pb.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include "canbus/canopen/sdo_request_protocol.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
|
|
||||||
#include "canbus/canopen/sync_protocol.h"
|
|
||||||
|
|
||||||
#include "canbus/canopen/nmt_response_protocol.h"
|
|
||||||
#include "canopen/nmt_request_protocol.h"
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace c701 {
|
|
||||||
class MotorController {
|
|
||||||
public:
|
|
||||||
MotorController(std::vector<uint8_t> node_ids):node_ids_(node_ids) {}
|
|
||||||
|
|
||||||
MotorController(const MotorController &) = delete;
|
|
||||||
|
|
||||||
MotorController &operator=(const MotorController &) = delete;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Destructor.
|
|
||||||
*/
|
|
||||||
virtual ~MotorController() = default;
|
|
||||||
|
|
||||||
msgs::ErrorCode Init(cmvr::device::AbstractCanbus *can_client,bool enable_log);
|
|
||||||
msgs::ErrorCode Start();
|
|
||||||
|
|
||||||
std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
|
|
||||||
auto data_ptr = std::make_unique<msgs::RobotDetail>();
|
|
||||||
message_manager_->GetSensorData(data_ptr.get());
|
|
||||||
return data_ptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void SeedNmtRequest(uint8_t node_id,msgs::NmtCommand command,uint32_t delay_ms = 10);
|
|
||||||
|
|
||||||
void SeedSdoRequest(uint8_t node_id,msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data,uint32_t delay_ms = 10);
|
|
||||||
|
|
||||||
|
|
||||||
// 设置软件限位（上限和下限）
|
|
||||||
void SetPositionLimits(uint8_t node_id, int32_t lower_limit, int32_t upper_limit);
|
|
||||||
void ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
|
|
||||||
void ConfigTPDO1(uint8_t node_id);
|
|
||||||
void ConfigTPDO2(uint8_t node_id);
|
|
||||||
|
|
||||||
// 目标位置 607A + 控制字 6040
|
|
||||||
void ConfigRPDO1(uint8_t node_id,bool start);
|
|
||||||
void SetTargetPosition(uint8_t node_id,double angle_rad);
|
|
||||||
void SetMode(uint8_t node_id,msgs::RunMode mode);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
|
||||||
static constexpr double GearRatio = 101.0; // 电机减速比
|
|
||||||
static constexpr double RADTODEG = 180.0 / 3.1415926;
|
|
||||||
std::vector<uint8_t> node_ids_{}; // 要控制的电机 id
|
|
||||||
// std::map<uint8_t,motor::Ti5MotorSdoRequestProtocol*> control_commands_{};
|
|
||||||
|
|
||||||
|
|
||||||
// nmt
|
|
||||||
device::NmtRequestProtocol<msgs::RobotDetail>* nmt_command_{nullptr};
|
|
||||||
|
|
||||||
//sync
|
|
||||||
device::SyncProtocol<msgs::RobotDetail>* sync_command_{nullptr};
|
|
||||||
|
|
||||||
// sdo
|
|
||||||
std::map<uint8_t,device::SdoRequestProtocol<msgs::RobotDetail>*> sdo_commands_{};
|
|
||||||
|
|
||||||
// rpdo1
|
|
||||||
std::map<uint8_t,robot::motor::Ti5MotorRPDO1*> rpdo1_commands_{};
|
|
||||||
|
|
||||||
|
|
||||||
std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
|
|
||||||
std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
|
|
||||||
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
|
|
||||||
|
|
||||||
msgs::RunMode cur_mode_{msgs::RunMode::RUN_MODE_UNSPECIFIED};
|
|
||||||
|
|
||||||
|
|
||||||
void SetPPTargetPosBySdo(uint8_t node_id,int32_t pos);
|
|
||||||
void SetPPTargetPosByPdo(uint8_t node_id,int32_t pos);
|
|
||||||
void SetCSPTargetPosByPdo(uint8_t node_id,int32_t pos);
|
|
||||||
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,86 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/18.
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
|
|
||||||
#include "robot/c701/motor/motor_controller.h"
|
|
||||||
#include "canbus/can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/message_manager.h"
|
|
||||||
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include "canbus/canopen/sdo_request_protocol.h"
|
|
||||||
#include "canbus/can_client/socket/socket_can_client_raw.h"
|
|
||||||
// #include "cmvr/msgs/ti5_motor.pb.h"
|
|
||||||
|
|
||||||
using cmvr::msgs::RobotDetail;
|
|
||||||
using cmvr::msgs::ErrorCode;
|
|
||||||
using cmvr::device::CanSender;
|
|
||||||
using cmvr::device::MessageManager;
|
|
||||||
|
|
||||||
using cmvr::robot::c701::MotorController;
|
|
||||||
using cmvr::device::SdoRequestProtocol;
|
|
||||||
using cmvr::device::SocketCanClientRaw;
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
|
|
||||||
TEST(MotorControllerTest, MotorControllerCmdTest) {
|
|
||||||
|
|
||||||
|
|
||||||
std::vector<uint8_t> node_ids = {0x03};
|
|
||||||
MotorController controller(node_ids);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
XmlNode xml_node;
|
|
||||||
SocketCanClientRaw can_client(xml_node);
|
|
||||||
can_client.start();
|
|
||||||
|
|
||||||
|
|
||||||
auto result = controller.Init(&can_client, false);
|
|
||||||
controller.Start();
|
|
||||||
|
|
||||||
for (uint8_t node_id: node_ids) {
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.ConfigTPDO1(node_id);
|
|
||||||
controller.ConfigTPDO2(node_id);
|
|
||||||
controller.ConfigRPDO1(node_id,true);
|
|
||||||
controller.ConfigProfile(node_id,1000,1000,1000);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.SeedNmtRequest(node_id,NMT_ENTER_PRE_OPERATIONAL);
|
|
||||||
|
|
||||||
controller.SeedNmtRequest(node_id,NMT_START_REMOTE_NODE);
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint8_t node_id: node_ids) {
|
|
||||||
controller.SetMode(node_id,RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.SetTargetPosition(node_id,3.14);
|
|
||||||
// controller.SyncStart();
|
|
||||||
// controller.SetTarget(node_id,3.14,1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
|
|
||||||
|
|
||||||
// controller.SetNmtRequest(test_id,NMT_RESET_NODE);
|
|
||||||
// controller.SeedSdoRequest(test_id,CS_READ_REQUEST,ACTUAL_POSITION_6064,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
EXPECT_EQ(result, ErrorCode::CANBUS_ERROR);
|
|
||||||
}
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/28.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
|
|
||||||
#include "glog/logging.h"
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
data[0] = target_pos_ & 0xFF;
|
|
||||||
data[1] = target_pos_ >> 8 & 0xFF;
|
|
||||||
data[2] = target_pos_ >> 16 & 0xFF;
|
|
||||||
data[3] = target_pos_ >> 24 & 0xFF;
|
|
||||||
|
|
||||||
data[4] = ctrl_word_ & 0xFF;
|
|
||||||
data[5] = ctrl_word_ >> 8 & 0xFF;
|
|
||||||
|
|
||||||
// data[4] = target_vel_ & 0xFF;
|
|
||||||
// data[5] = target_vel_ >> 8 & 0xFF;
|
|
||||||
// data[6] = target_vel_ >> 16 & 0xFF;
|
|
||||||
// data[7] = target_vel_ >> 24 & 0xFF;
|
|
||||||
}
|
|
||||||
@ -1,60 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/28.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include <mutex>
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorRPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::RPDO1_BASE_ID_200;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorRPDO1(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void UpdateData(uint8_t *data) override;
|
|
||||||
|
|
||||||
int32_t GetLength() const override {
|
|
||||||
return 0x06;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t GetPeriod() const override {
|
|
||||||
return 1000 * 1; // 5 ms
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetTargetPos(int32_t position) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
target_pos_ = position;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetTargetVel(int32_t velocity) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
target_vel_ = velocity;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetCtrlWord(uint16_t ctrl_word) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
ctrl_word_ = ctrl_word;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
mutable std::mutex mutex_;
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
int32_t target_pos_{0};
|
|
||||||
int32_t target_vel_{0};
|
|
||||||
|
|
||||||
uint16_t ctrl_word_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,39 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/24.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_sdo_response.h"
|
|
||||||
|
|
||||||
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
|
|
||||||
cmvr::msgs::RobotDetail *sensor_data) const {
|
|
||||||
// 通过 node_id 获取对应的电机状态（Ti5_MotorStatus）
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
// 先把基础的 sdo_response 拷贝进去
|
|
||||||
motor_status->mutable_sdo_response()->CopyFrom(sdo_response);
|
|
||||||
|
|
||||||
|
|
||||||
switch (sdo_response.index()) {
|
|
||||||
case msgs::CONTROL_WORD_6040:
|
|
||||||
motor_status->set_ctrl_word(sdo_response.data());
|
|
||||||
break;
|
|
||||||
case msgs::STATUS_WORD_6041:
|
|
||||||
motor_status->set_status_word(sdo_response.data());
|
|
||||||
break;
|
|
||||||
case msgs::ACTUAL_POSITION_6064:
|
|
||||||
motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
|
|
||||||
LOG(INFO) << "pos = " << motor_status->position();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
|
|
||||||
<< ": command=" << int(sdo_response.cs())
|
|
||||||
<< ", index=" << sdo_response.index()
|
|
||||||
<< ", subindex=" << int(sdo_response.sub_index())
|
|
||||||
<< ", data=" << sdo_response.data();
|
|
||||||
}
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/24.
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/canopen/sdo_response_protocol.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorSdoResponse : public device::SdoResponseProtocol<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
explicit Ti5MotorSdoResponse(uint8_t node_id)
|
|
||||||
: SdoResponseProtocol<cmvr::msgs::RobotDetail>(node_id) {
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
|
||||||
void ParseSdoData(const msgs::SdoFrame &sdo_response,
|
|
||||||
cmvr::msgs::RobotDetail *sensor_data) const override;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,36 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_tpdo1.h"
|
|
||||||
#include "glog/logging.h"
|
|
||||||
#include "canbus/canopen/register.h"
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
|
|
||||||
if (length < 7) {
|
|
||||||
LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
motor_status->set_ctrl_word(bytes[1] << 8 | bytes[0]);
|
|
||||||
motor_status->set_status_word(bytes[3] << 8 | bytes[2]);
|
|
||||||
motor_status->set_run_mode(static_cast<RunMode>(bytes[4]));
|
|
||||||
motor_status->set_error_state(bytes[6] << 8 | bytes[5]);
|
|
||||||
|
|
||||||
statusword_t st{};
|
|
||||||
st.value = motor_status->status_word();
|
|
||||||
|
|
||||||
if (st.op_mode_specific > 0) {
|
|
||||||
LOG(INFO) << st.op_mode_specific ;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorTPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::TPDO1_BASE_ID_180;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorTPDO1(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_tpdo2.h"
|
|
||||||
#include "glog/logging.h"
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
|
|
||||||
if (length < 8) {
|
|
||||||
LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
|
|
||||||
motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
|
|
||||||
|
|
||||||
LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed();
|
|
||||||
}
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorTPDO2 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::TPDO2_BASE_ID_280;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorTPDO2(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -8,7 +8,7 @@
|
|||||||
#include <curl/curl.h>
|
#include <curl/curl.h>
|
||||||
|
|
||||||
#include "rapidxml/xml_parser.h"
|
#include "rapidxml/xml_parser.h"
|
||||||
|
#include "jsoncpp/json/json.h"
|
||||||
namespace cmvr::device
|
namespace cmvr::device
|
||||||
{
|
{
|
||||||
enum ControllerState
|
enum ControllerState
|
||||||
@ -25,7 +25,7 @@ namespace cmvr::device
|
|||||||
|
|
||||||
[[nodiscard]] ControllerState getState() const {return state_;}
|
[[nodiscard]] ControllerState getState() const {return state_;}
|
||||||
|
|
||||||
virtual void call(std::string json) = 0;
|
virtual void call(const Json::Value& json) = 0;
|
||||||
|
|
||||||
virtual void stop() = 0;
|
virtual void stop() = 0;
|
||||||
|
|
||||||
|
|||||||
@ -12,7 +12,7 @@ CartesianController::CartesianController(const XmlNode& cfg):AbstractController(
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CartesianController::call(std::string json)
|
void CartesianController::call(const Json::Value& json)
|
||||||
{
|
{
|
||||||
if (state_ != ControllerState_Idle)
|
if (state_ != ControllerState_Idle)
|
||||||
return;
|
return;
|
||||||
|
|||||||
@ -12,9 +12,10 @@ namespace cmvr::device
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
explicit CartesianController(const XmlNode& cfg);
|
explicit CartesianController(const XmlNode& cfg);
|
||||||
void call(std::string json) override;
|
void call(const Json::Value& json) override;
|
||||||
void stop() override;
|
void stop() override;
|
||||||
private:
|
private:
|
||||||
|
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -28,9 +28,9 @@ void ControllerManager::create(const XmlNode& cfg)
|
|||||||
{
|
{
|
||||||
ComponentGroup group;
|
ComponentGroup group;
|
||||||
group.id = child.getAttrString("id");
|
group.id = child.getAttrString("id");
|
||||||
// CanGroupID可能记录了多个cangroupID
|
// CanGroupID可能记录了多个canGroupID
|
||||||
std::string cangroupIDs = child.getAttrString("CanGroupID");
|
std::string canGroupIDs = child.getAttrString("CanGroupID");
|
||||||
std::istringstream iss(cangroupIDs);
|
std::istringstream iss(canGroupIDs);
|
||||||
std::string id;
|
std::string id;
|
||||||
while (std::getline(iss, id, ',')) {
|
while (std::getline(iss, id, ',')) {
|
||||||
group.CanGroupIDs.push_back(id);
|
group.CanGroupIDs.push_back(id);
|
||||||
@ -47,6 +47,7 @@ void ControllerManager::create(const XmlNode& cfg)
|
|||||||
group.controllers_["CartesianController"] = std::make_shared<CartesianController>(controller_node);
|
group.controllers_["CartesianController"] = std::make_shared<CartesianController>(controller_node);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
//
|
||||||
|
|
||||||
componentGroups_[group.id] = group;
|
componentGroups_[group.id] = group;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -37,7 +37,7 @@ namespace cmvr::device
|
|||||||
// 清除错误信息
|
// 清除错误信息
|
||||||
void clearError();
|
void clearError();
|
||||||
|
|
||||||
|
// 根据xml内容创建控制器组
|
||||||
void create(const XmlNode& cfg);
|
void create(const XmlNode& cfg);
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|||||||
@ -11,7 +11,7 @@ JointPositionController::JointPositionController(const XmlNode& cfg):AbstractCon
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void JointPositionController::call(std::string json)
|
void JointPositionController::call(const Json::Value& json)
|
||||||
{
|
{
|
||||||
if (state_ != ControllerState_Idle)
|
if (state_ != ControllerState_Idle)
|
||||||
return;
|
return;
|
||||||
|
|||||||
@ -12,7 +12,7 @@ namespace cmvr::device
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
explicit JointPositionController(const XmlNode& cfg);
|
explicit JointPositionController(const XmlNode& cfg);
|
||||||
void call(std::string json) override;
|
void call(const Json::Value& json) override;
|
||||||
void stop() override;
|
void stop() override;
|
||||||
private:
|
private:
|
||||||
};
|
};
|
||||||
|
|||||||
@ -1,5 +0,0 @@
|
|||||||
#add_library(ti5robot SHARED ti5_robot.cpp)
|
|
||||||
#
|
|
||||||
#target_include_directories(ti5robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
||||||
#
|
|
||||||
#add_library(cmvr_es::device::ti5robot ALIAS ti5robot)
|
|
||||||
@ -1,343 +0,0 @@
|
|||||||
//#include "ti5_robot.h"
|
|
||||||
//#include <string>
|
|
||||||
//#include <vector>
|
|
||||||
//#include <memory>
|
|
||||||
//#include <chrono>
|
|
||||||
//#include <thread>
|
|
||||||
//#include <sstream>
|
|
||||||
//#include <cstring>
|
|
||||||
//#include <sys/socket.h>
|
|
||||||
//#include <arpa/inet.h>
|
|
||||||
//#include <unistd.h>
|
|
||||||
//#include <csignal>
|
|
||||||
//#include <mutex>
|
|
||||||
//#include <shared_mutex>
|
|
||||||
//#include <termios.h>
|
|
||||||
//#include <fcntl.h>
|
|
||||||
//#include <algorithm>
|
|
||||||
//#include <sys/socket.h>
|
|
||||||
//#include <nlohmann/json.hpp>
|
|
||||||
//
|
|
||||||
//using json = nlohmann::json;
|
|
||||||
//using namespace std;
|
|
||||||
//using namespace cmvr::device;
|
|
||||||
//// 服务端地址和端口（需与服务端保持一致）
|
|
||||||
//#define SERVER_IP "10.148.108.142" // 服务端IP，本地测试用127.0.0.1
|
|
||||||
//#define SERVER_PORT 8888 // 服务端端口，与服务端PORT一致
|
|
||||||
//// 设置socket超时（单位：毫秒）
|
|
||||||
//void set_socket_timeout(int sock, long timeout_ms) {
|
|
||||||
// struct timeval tv;
|
|
||||||
// tv.tv_sec = timeout_ms / 1000;
|
|
||||||
// tv.tv_usec = (timeout_ms % 1000) * 1000;
|
|
||||||
// setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//// 发送请求并接收响应的函数 正常返回应该是Json字符串
|
|
||||||
//bool send_request(const string& request_str,string& result) {
|
|
||||||
// int sock = 0;
|
|
||||||
// struct sockaddr_in serv_addr;
|
|
||||||
// char buffer[1024] = {0};
|
|
||||||
//
|
|
||||||
// // 创建socket
|
|
||||||
// if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] (send_request): socket create failed";
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// // 设置服务端地址信息
|
|
||||||
// serv_addr.sin_family = AF_INET;
|
|
||||||
// serv_addr.sin_port = htons(SERVER_PORT);
|
|
||||||
// if (inet_pton(AF_INET, SERVER_IP, &serv_addr.sin_addr) <= 0) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] (send_request): invaid ip address";
|
|
||||||
// close(sock);
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// // 连接服务端
|
|
||||||
// if (connect(sock, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] (send_request): connect server failed";
|
|
||||||
// close(sock);
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// // 发送JSON请求
|
|
||||||
// send(sock, request_str.c_str(), request_str.length(), 0);
|
|
||||||
// LOG(INFO) << "[Ti5Robot] (send_request): request info:" << request_str;
|
|
||||||
// // 设置接收超时为3秒
|
|
||||||
// set_socket_timeout(sock, 2000);
|
|
||||||
// // 接收服务端响应
|
|
||||||
// ssize_t valread = read(sock, buffer, 1024);
|
|
||||||
//
|
|
||||||
// close(sock); // 关闭连接
|
|
||||||
// if (valread == -1 && errno == EAGAIN) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] (send_request): timeout";
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
// else if (valread <= 0) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] (send_request): read error:" << valread;
|
|
||||||
// return false;
|
|
||||||
// }
|
|
||||||
// else {
|
|
||||||
// result = buffer;
|
|
||||||
// }
|
|
||||||
// return true;
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//// Ti5Robot 类的实现
|
|
||||||
//Ti5Robot::Ti5Robot(const XmlNode& cfg) : AbstractRobot(cfg) {
|
|
||||||
// // 从配置中初始化机器人参数
|
|
||||||
// try {
|
|
||||||
// id_ = cfg_.getAttrString("id");
|
|
||||||
// if (id_ == "left_arm") {
|
|
||||||
// arm_ = Left_Arm;
|
|
||||||
// } else if (id_ == "right_arm") {
|
|
||||||
// arm_ = Right_Arm;
|
|
||||||
// } else {
|
|
||||||
// //报错
|
|
||||||
// }
|
|
||||||
// canInd_ = cfg_.getAttrDefault("canInd", 1);
|
|
||||||
// deviceInd_ = cfg_.getAttrDefault("deviceInd", 1);
|
|
||||||
// }
|
|
||||||
// catch (const exception& e) {
|
|
||||||
// LOG(ERROR) << "[Ti5Robot] ([Ti5Robot]): Failed to parse XML: " << e.what();
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//Ti5Robot::~Ti5Robot() {
|
|
||||||
// LOG(INFO) << "Ti5Robot 析构函数被调用";
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::init() {
|
|
||||||
// // 连接socket服务
|
|
||||||
// // 检查是否可以连接到服务端。
|
|
||||||
// // 角度范围应该在[-π,π]之间？
|
|
||||||
// std::vector<double> goal_j(7,0.5);
|
|
||||||
// moveJ(goal_j,0.5,0.1);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::start() {
|
|
||||||
// LOG(INFO) << "Ti5Robot 启动";
|
|
||||||
// // 启动机器人运行
|
|
||||||
// // 可以添加启动CAN通道等操作
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::stop() {
|
|
||||||
// LOG(INFO) << "Ti5Robot 停止";
|
|
||||||
// // 停止机器人运行
|
|
||||||
// // 可以添加停止CAN通道等操作
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::getState(RobotState &state) {
|
|
||||||
// // 获取机器人当前状态
|
|
||||||
// LOG(INFO) << "[Ti5Robot](getState)";
|
|
||||||
//
|
|
||||||
// // 获取左右臂的关节角度
|
|
||||||
// float currentAngles[7];
|
|
||||||
// get_current_angle(arm_,currentAngles,canInd_,deviceInd_);
|
|
||||||
// for (float currentAngle : currentAngles)
|
|
||||||
// {
|
|
||||||
// state.joint_positions.push_back(currentAngle);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// // 获取末端执行器位姿
|
|
||||||
//// float pos[6];
|
|
||||||
//// get_current_pose(Left_Arm, pos, 0, 0);
|
|
||||||
//// state.ee_pose.position.x = pos[0];
|
|
||||||
//// state.ee_pose.position.y = pos[1];
|
|
||||||
//// state.ee_pose.position.z = pos[2];
|
|
||||||
//// state.ee_pose.euler.rx = pos[3];
|
|
||||||
//// state.ee_pose.euler.ry = pos[4];
|
|
||||||
//// state.ee_pose.euler.rz = pos[5];
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::eStop() {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::eStop]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::eStop]: The current interface is not yet implemented.");
|
|
||||||
// LOG(INFO) << "Ti5Robot 紧急停止";
|
|
||||||
// // 实现紧急停止逻辑
|
|
||||||
// // 例如刹车操作
|
|
||||||
// // brake(LEFT_ARM, 0, 0); // 假设使用左臂，设备索引0，通道索引0
|
|
||||||
// // brake(RIGHT_ARM, 0, 0);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::moveJ(std::vector<double> &joints, double vel, double acc) {
|
|
||||||
// //LOG(WARNING) << "[Ti5Robot::moveJ]: The current interface is not yet implemented.";
|
|
||||||
// //throw std::runtime_error("[Ti5Robot::moveJ]: The current interface is not yet implemented.");
|
|
||||||
// LOG(INFO) << "Ti5Robot 关节空间运动: 关节角度=";
|
|
||||||
// // 实现关节空间运动控制逻辑
|
|
||||||
// float goal_j[7];
|
|
||||||
// for (int i = 0; i < 7; ++i) {
|
|
||||||
// goal_j[i] = static_cast<float>(joints[i]);
|
|
||||||
// }
|
|
||||||
// joint_to_move(arm_,goal_j,deviceInd_,canInd_);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::moveJ_IK(math::Pose3d &pose, double vel, double acc) {
|
|
||||||
// float pos[6] = {static_cast<float>(pose.position.x), static_cast<float>(pose.position.y), static_cast<float>(pose.position.z),
|
|
||||||
// static_cast<float>(pose.euler.rx), static_cast<float>(pose.euler.ry), static_cast<float>(pose.euler.rz)};
|
|
||||||
// pos_to_move(arm_, pos, 0, 0);
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::moveL(math::Pose3d &pose, double vel, double acc) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::moveL]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::moveL]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::teachJ(RobotJointIndex joint, RobotJointIndexDirection dir, double vel, double acc) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::teachJ]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::teachJ]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::teachL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::teachL]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::teachL]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::followPoseTrajectory]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::servoJ(std::vector<double> &joints, double dt) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::servoJ]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::servoJ]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//void Ti5Robot::servoL(math::Pose3d &pose, double dt) {
|
|
||||||
// LOG(WARNING) << "[Ti5Robot::servoL]: The current interface is not yet implemented.";
|
|
||||||
// throw std::runtime_error("[Ti5Robot::servoL]: The current interface is not yet implemented.");
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//bool Ti5Robot::get_current_angle(ArmType arm, float goal_j[7], int canInd, int deviceInd)
|
|
||||||
//{
|
|
||||||
// // 获取关节位置
|
|
||||||
// // 获取当前关节角度
|
|
||||||
// bool success = false;
|
|
||||||
// json req;
|
|
||||||
// req["interface"] = "get_current_angle"; // 接口名
|
|
||||||
// if (arm == Left_Arm) {
|
|
||||||
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// else if (arm == Right_Arm) {
|
|
||||||
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
|
|
||||||
// req["parameters"]["canInd"] = canInd; // CAN通道号
|
|
||||||
//
|
|
||||||
// string resp_str;
|
|
||||||
// auto ret = send_request(req.dump(),resp_str);
|
|
||||||
// if (ret && !resp_str.empty()) {
|
|
||||||
// json resp = json::parse(resp_str);
|
|
||||||
// if (resp.contains("error")) {
|
|
||||||
// cerr << "接口调用失败: " << resp["error"] << endl;
|
|
||||||
// } else {
|
|
||||||
// std::vector<float> angeles = resp["result"];
|
|
||||||
// for (int i = 0; i < 7; ++i) {
|
|
||||||
// goal_j[i] = angeles[i];
|
|
||||||
// success = true;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// return success;
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//bool Ti5Robot::get_current_pose(ArmType arm, float pos[6], int canInd, int deviceInd)
|
|
||||||
//{
|
|
||||||
// // 获取关节位置
|
|
||||||
// // 获取当前关节角度
|
|
||||||
// bool success = false;
|
|
||||||
// json req;
|
|
||||||
// req["interface"] = "get_current_pose"; // 接口名
|
|
||||||
// if (arm == Left_Arm) {
|
|
||||||
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// else if (arm == Right_Arm) {
|
|
||||||
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
|
|
||||||
// req["parameters"]["canInd"] = canInd; // CAN通道号
|
|
||||||
//
|
|
||||||
// string resp_str;
|
|
||||||
// auto ret = send_request(req.dump(),resp_str);
|
|
||||||
// if (ret && !resp_str.empty()) {
|
|
||||||
// json resp = json::parse(resp_str);
|
|
||||||
// if (resp.contains("error")) {
|
|
||||||
// cerr << "接口调用失败: " << resp["error"] << endl;
|
|
||||||
// } else {
|
|
||||||
// std::vector<float> result = resp["result"];
|
|
||||||
// for (int i = 0; i < 6; ++i) {
|
|
||||||
// pos[i] = result[i];
|
|
||||||
// success = true;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// return success;
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//
|
|
||||||
//bool Ti5Robot::joint_to_move(ArmType side, float *goal_j, int deviceInd, int canInd)
|
|
||||||
//{
|
|
||||||
// bool success = false;
|
|
||||||
//
|
|
||||||
// json req;
|
|
||||||
// req["interface"] = "joint_to_move"; // 接口名
|
|
||||||
// if (arm_ == Left_Arm) {
|
|
||||||
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// else if (arm_ == Right_Arm) {
|
|
||||||
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// req["parameters"]["deviceInd"] = deviceInd; // CAN设备号
|
|
||||||
// req["parameters"]["canInd"] = canInd; // CAN通道号
|
|
||||||
// // 目标角度数组（7个关节角度，单位：弧度）
|
|
||||||
// req["parameters"]["goal_j"] = *goal_j;
|
|
||||||
//
|
|
||||||
// string resp_str;
|
|
||||||
// auto ret = send_request(req.dump(),resp_str);
|
|
||||||
// if (ret && !resp_str.empty()) {
|
|
||||||
// json resp = json::parse(resp_str);
|
|
||||||
// if (resp.contains("error")) {
|
|
||||||
// success = false;
|
|
||||||
// } else {
|
|
||||||
// success = true;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// return success;
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
//bool Ti5Robot::pos_to_move(ArmType side, float *pos, int deviceInd, int canInd)
|
|
||||||
//{
|
|
||||||
// bool success = false;
|
|
||||||
// json req;
|
|
||||||
// req["interface"] = "pos_to_move"; // 接口名
|
|
||||||
// if (arm_ == Left_Arm) {
|
|
||||||
// req["parameters"]["side"] = "LEFT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// else if (arm_ == Right_Arm) {
|
|
||||||
// req["parameters"]["side"] = "RIGHT_ARM"; // 参数：右臂
|
|
||||||
// }
|
|
||||||
// req["parameters"]["deviceInd"] = deviceInd_; // CAN设备号
|
|
||||||
// req["parameters"]["canInd"] = canInd_; // CAN通道号
|
|
||||||
// req["parameters"]["pos"] = *pos;
|
|
||||||
//
|
|
||||||
// string resp_str;
|
|
||||||
// auto ret = send_request(req.dump(),resp_str);
|
|
||||||
// if (ret && !resp_str.empty()) {
|
|
||||||
// json resp = json::parse(resp_str);
|
|
||||||
// if (resp.contains("error")) {
|
|
||||||
// success = false;
|
|
||||||
// } else {
|
|
||||||
// success = true;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
// return success;
|
|
||||||
//}
|
|
||||||
@ -1,49 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by linbo on 2025/7/1.
|
|
||||||
//
|
|
||||||
|
|
||||||
#ifndef TI5_ROBOT_H
|
|
||||||
#define TI5_ROBOT_H
|
|
||||||
|
|
||||||
#include "devices/abstract_robot.h"
|
|
||||||
|
|
||||||
namespace cmvr::device {
|
|
||||||
class Ti5Robot final : public AbstractRobot
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
explicit Ti5Robot(const XmlNode& cfg);
|
|
||||||
~Ti5Robot() override;
|
|
||||||
void init() override;
|
|
||||||
void start() override;
|
|
||||||
void stop() override;
|
|
||||||
|
|
||||||
void getState(RobotState &state) override;
|
|
||||||
void eStop() override;
|
|
||||||
void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) override;
|
|
||||||
void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) override;
|
|
||||||
void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) override;
|
|
||||||
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) override;
|
|
||||||
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) override;
|
|
||||||
void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) override;
|
|
||||||
void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) override;
|
|
||||||
void servoJ(std::vector<double> &joints, double dt) override;
|
|
||||||
void servoL(math::Pose3d &pose, double dt) override;
|
|
||||||
|
|
||||||
|
|
||||||
// 以下为socket调用ti5robot sdk的接口
|
|
||||||
// bool get_current_angle(ArmType arm, float goal_j[7], int canInd,int deviceInd);
|
|
||||||
// bool get_current_pose(ArmType arm, float pos[6], int canInd, int deviceInd);
|
|
||||||
// bool joint_to_move(ArmType side, float *goal_j, int deviceInd, int canInd);
|
|
||||||
// bool pos_to_move(ArmType side, float *pos, int deviceInd, int canInd);
|
|
||||||
private:
|
|
||||||
// ArmType arm_;
|
|
||||||
// int canInd_;
|
|
||||||
// int deviceInd_;
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif //TI5_ROBOT_H
|
|
||||||
@ -1,252 +0,0 @@
|
|||||||
#ifndef Ti5BASIC_H
|
|
||||||
#define Ti5BASIC_H
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <ifaddrs.h>
|
|
||||||
#include <netinet/in.h>
|
|
||||||
#include <arpa/inet.h>
|
|
||||||
#include <iostream>
|
|
||||||
#include <fstream>
|
|
||||||
#include <string>
|
|
||||||
#include <unordered_map>
|
|
||||||
#include <sstream>
|
|
||||||
#include <map>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <fcntl.h>
|
|
||||||
#include <termios.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include <sys/mman.h>
|
|
||||||
#include <vector>
|
|
||||||
#include <regex>
|
|
||||||
#include <iomanip>
|
|
||||||
#include <cstring>
|
|
||||||
|
|
||||||
#include "Ti5LOGIC.h"
|
|
||||||
#include "Ti5MOVE.h"
|
|
||||||
|
|
||||||
// #include "can/tcontrolcanfactor.h"
|
|
||||||
|
|
||||||
#define MAX_IP_ADDR_LEN 256 // 存储IP地址的最大长度
|
|
||||||
|
|
||||||
|
|
||||||
// extern float arr_s[6];
|
|
||||||
|
|
||||||
extern bool flag;
|
|
||||||
|
|
||||||
extern string log_path; // log文件
|
|
||||||
|
|
||||||
extern char LogInfo[100]; // 存储写入log文件的信息
|
|
||||||
|
|
||||||
extern char Info_Str[100]; // 定义一个字符数组用于存储 flag 的字符串表示
|
|
||||||
class ArmController; // 声明ArmController类
|
|
||||||
|
|
||||||
extern uint8_t l_id[IDNUM], r_id[IDNUM]; // 声明左臂和右臂的canID数组
|
|
||||||
extern ArmController l_controller; // 声明TH_L对象
|
|
||||||
extern ArmController r_controller; // 声明TH_R对象
|
|
||||||
|
|
||||||
extern class humanoidLeftArm l_solver; // 数学上的解算器
|
|
||||||
extern class humanoidRightArm r_solver; // 解算器
|
|
||||||
|
|
||||||
|
|
||||||
enum ArmSide
|
|
||||||
{
|
|
||||||
LEFT_ARM, // 左臂
|
|
||||||
RIGHT_ARM // 右臂
|
|
||||||
};
|
|
||||||
|
|
||||||
extern "C"
|
|
||||||
{ // 添加extern "C"
|
|
||||||
|
|
||||||
// 写入调试信息到文件
|
|
||||||
void writeDebugInfoToFile(const char *func_name, const char *info);
|
|
||||||
|
|
||||||
// 输出数组的调试信息
|
|
||||||
void printArrayDebugInfo(float arr[], int size, const char *arr_name);
|
|
||||||
|
|
||||||
/* 获取本机IP地址*/
|
|
||||||
std::string ip_address();
|
|
||||||
|
|
||||||
/*std::string query_can();
|
|
||||||
查询can设备号
|
|
||||||
*/
|
|
||||||
std::vector<std::string> query_can();
|
|
||||||
|
|
||||||
void receive_fully(int sock, void *buffer, size_t size);
|
|
||||||
/*socket通信
|
|
||||||
参数：
|
|
||||||
deviceInd：can设备号
|
|
||||||
canInd：can通道
|
|
||||||
port：端口号
|
|
||||||
发送端发送数据格式：{标识位，数据}
|
|
||||||
标识位
|
|
||||||
0x01 左臂pos
|
|
||||||
0x02 右臂pos
|
|
||||||
0x03 左臂joint
|
|
||||||
0x04 右臂joint
|
|
||||||
*/
|
|
||||||
int ti5_socket_server(int deviceInd, int canInd,int port);
|
|
||||||
|
|
||||||
/*获取电机错误状态
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
deviceInd：can设备号
|
|
||||||
canInd：can通道
|
|
||||||
dataList:接收数据的数组，错误状态
|
|
||||||
返回值：为电机错误
|
|
||||||
0：无错误
|
|
||||||
1：软件错误
|
|
||||||
2：过压
|
|
||||||
4：欠压
|
|
||||||
16：启动错误
|
|
||||||
*/
|
|
||||||
// int get_mechanicalarm_Maschinen_status(ArmSide side, int deviceInd, int canInd);
|
|
||||||
void get_mechanicalarm_status(ArmSide side, int deviceInd, int canInd, int32_t *dataList);
|
|
||||||
|
|
||||||
/*清除电机错误
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
deviceInd：can设备号
|
|
||||||
canInd：can通道
|
|
||||||
*/
|
|
||||||
void clear_elc_error(ArmSide side, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*机械臂刹车
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
返回值：
|
|
||||||
true：成功
|
|
||||||
false：失败
|
|
||||||
*/
|
|
||||||
bool brake(ArmSide side, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*将数据记录下来写入文件
|
|
||||||
参数：
|
|
||||||
pj_flag：角度或者位姿标识,1为角度 ，0为坐标
|
|
||||||
filename：存储文件名
|
|
||||||
array[6]：被保存的值
|
|
||||||
*/
|
|
||||||
void write_value(int pj_flag, string filename, float array[7]);
|
|
||||||
|
|
||||||
/*机械臂回到原点
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
返回值：无
|
|
||||||
*/
|
|
||||||
void mechanical_arm_origin(ArmSide side, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*机械臂关节运动
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
goal_j：目标关节角
|
|
||||||
canInd：can通道
|
|
||||||
DeviceInd：can设备号
|
|
||||||
返回值：无*/
|
|
||||||
void joint_to_move(ArmSide side, float *goal_j, int deviceInd, int canInd);
|
|
||||||
void new_joint_to_move(ArmSide side, float *goal_j, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*机械臂关节运动，同时获取当前位置
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
goal_j：目标关节角
|
|
||||||
CUrrentJointPosition：存储当前位置
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
返回值：无
|
|
||||||
*/
|
|
||||||
void GetP_joint_to_move(ArmSide side, float *goal_j, float *CUrrentJointPosition, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*pos运动
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
pos：目标位置
|
|
||||||
value：dim的值
|
|
||||||
dim：-1~2 代表x,y,z， -1的时候是没有臂角约束，只会接收末端位姿，其他参数忽略
|
|
||||||
absolute：true的时候是绝对位置（以胸部原点位置），false的时候是相对位置（以当前点胳膊轴位置）
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
返回值：无
|
|
||||||
*/
|
|
||||||
// void pos_to_move(ArmSide side, float *pos, float value, int dim, bool absolute, int deviceInd, int canInd);
|
|
||||||
bool pos_to_move(ArmSide side, float *pos, int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*获取当前角度
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
goal_j：存储角度的数组
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
*/
|
|
||||||
void get_current_angle(ArmSide side, float goal_j[7], int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*获取当前位姿
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
posz：存储位姿的数组
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
*/
|
|
||||||
void get_current_pose(ArmSide side, float posz[7], int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*设置为电流模式，并设置目标电流
|
|
||||||
参数：
|
|
||||||
side：左臂或右臂 （LEFT_ARM 左臂，RIGHT_ARM 右臂）
|
|
||||||
current: 目标电流
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
*/
|
|
||||||
void set_current_mode(ArmSide side, uint32_t current[7], int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*设置键盘控制模式
|
|
||||||
参数：
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
*/
|
|
||||||
void LLL_keyboard_controller_J(int deviceInd, int canInd);
|
|
||||||
void LLL_keyboard_controller_P(int deviceInd, int canInd);
|
|
||||||
void RRR_keyboard_controller_J(int deviceInd, int canInd);
|
|
||||||
void RRR_keyboard_controller_P(int deviceInd, int canInd);
|
|
||||||
// void L_keyboard_controller(int deviceInd, int canInd);
|
|
||||||
// void R_keyboard_controller(int deviceInd, int canInd);
|
|
||||||
|
|
||||||
/*获取电机当前位置
|
|
||||||
参数：
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
MotorsTotal：电机数量
|
|
||||||
MotorsIDlist：电机ID列表
|
|
||||||
data：存储电机位置数据的数组
|
|
||||||
*/
|
|
||||||
void get_motor_position(int deviceInd, int canInd, int MotorsTotal,uint8_t *MotorsIDlist,int32_t *MotorPosition);
|
|
||||||
|
|
||||||
/*设置电机当前位置
|
|
||||||
参数：
|
|
||||||
canInd：can通道
|
|
||||||
deviceInd：can设备号
|
|
||||||
MotorsTotal：电机数量
|
|
||||||
MotorsIDlist：电机ID列表
|
|
||||||
data：存储电机位置数据的数组
|
|
||||||
*/
|
|
||||||
void set_motor_position(int deviceInd, int canInd, int MotorsTotal,uint8_t *MotorsIDlist,int32_t *MotorPosition);
|
|
||||||
|
|
||||||
|
|
||||||
// 设定终端为非阻塞模式
|
|
||||||
void setNonBlocking(bool enable) ;
|
|
||||||
|
|
||||||
// 设定文件描述符为非阻塞
|
|
||||||
void setNonBlockingInput() ;
|
|
||||||
// 清空键盘缓冲区
|
|
||||||
void clearStdinBuffer();
|
|
||||||
|
|
||||||
// 读取键盘输入
|
|
||||||
char ssscanKeyboard();
|
|
||||||
|
|
||||||
} // 添加extern "C"
|
|
||||||
#endif
|
|
||||||
@ -1,171 +0,0 @@
|
|||||||
#ifndef TI5CAN_DRIVER_H
|
|
||||||
#define TI5CAN_DRIVER_H
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <string>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <vector>
|
|
||||||
#include <map>
|
|
||||||
#include <algorithm>
|
|
||||||
#include <unordered_map>
|
|
||||||
|
|
||||||
|
|
||||||
#include "controlcan.h"
|
|
||||||
#include "Ti5BASIC.h"
|
|
||||||
|
|
||||||
#define GET_MOTOR_PERSIONS 8 //获取电机当前位置指令（1字节），转化为减速机角度公式：(返回值/65536/减速比)*360
|
|
||||||
#define GET_MOTOR_ERROR 10 //获取电机错误指令（1字节）
|
|
||||||
#define CLEAR_ERROR 11 //清除电机错误指令（1字节）
|
|
||||||
#define SET_MOTOR_CURRENTS 28 //设置电机为电流模式，并设置目标电流指令（5字节）
|
|
||||||
#define SET_MOTOR_SPEED 29 //设置电机为速度模式，并设置目标速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
|
|
||||||
#define SET_MOTOR_POSITION 30 //设置电机为位置模式，并设置目标位置指令（5字节），下发参数为：(减速机目标角度/360)*减速比*65536
|
|
||||||
#define SET_MOTOR_MAX_SPEED 36 //设置电机最大正向允许速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
|
|
||||||
#define SET_MOTOR_MIN_SPEED 37 //设置电机最小负向允许速度指令（5字节），下发参数为：(目标转速（度每秒）*减速比*100)/360
|
|
||||||
|
|
||||||
|
|
||||||
extern int CanNum;
|
|
||||||
|
|
||||||
// 查询 CAN 设备并绑定序列号与设备索引
|
|
||||||
std::map<std::string, int> query_can_with_index();
|
|
||||||
/*登录并初始化can设备
|
|
||||||
参数：无
|
|
||||||
返回值
|
|
||||||
true：成功
|
|
||||||
false：失败
|
|
||||||
*/
|
|
||||||
bool Start();
|
|
||||||
// void Start();
|
|
||||||
|
|
||||||
bool compare_serial_numbers(const std::string &a, const std::string &b);
|
|
||||||
std::unordered_map<std::string, int> get_device_map();
|
|
||||||
void operate_multiple_can_devices();
|
|
||||||
|
|
||||||
/*登出can设备
|
|
||||||
参数：无
|
|
||||||
返回值
|
|
||||||
true：成功
|
|
||||||
false：失败
|
|
||||||
*/
|
|
||||||
bool Exit();
|
|
||||||
|
|
||||||
int32_t convertHexArrayToDecimal(const uint8_t hexArray[4]);
|
|
||||||
|
|
||||||
void toIntArray(int number, int *res, int size);
|
|
||||||
|
|
||||||
/*发送1字节指令
|
|
||||||
参数：
|
|
||||||
DeviceInd：can设备索引 （一个为0,2个为1）
|
|
||||||
CANInd：can通道索引 （通道1：0,通道2：1）
|
|
||||||
numOfActuator：发送指令的电机数量
|
|
||||||
canIdList：电机canId列表
|
|
||||||
commandList：指令列表
|
|
||||||
dataList：接收数据列表
|
|
||||||
*/
|
|
||||||
void sendSimpleCanCommand(int DeviceInd, int CANInd,uint8_t numOfActuator, uint8_t *canIdList, uint8_t command,int32_t *dataList);
|
|
||||||
|
|
||||||
void sendCanCommand(int DeviceInd,int CANInd,uint8_t numOfActuator, uint8_t *canIdList, uint8_t command, uint32_t *parameterList);
|
|
||||||
#endif // TEST_CAN_H
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// // 比较函数，用于按字典序排序设备序列号
|
|
||||||
// bool compare_serial_numbers(const std::string &a, const std::string &b)
|
|
||||||
// {
|
|
||||||
// return a < b; // 字典序比较
|
|
||||||
// }
|
|
||||||
|
|
||||||
// std::unordered_map<std::string, int> get_device_map()
|
|
||||||
// {
|
|
||||||
// VCI_BOARD_INFO pinfo[50];
|
|
||||||
// std::unordered_map<std::string, int> deviceMap;
|
|
||||||
// int num = VCI_FindUsbDevice2(pinfo); // 查询所有CAN设备
|
|
||||||
|
|
||||||
// // 提取所有设备的序列号
|
|
||||||
// std::vector<std::string> serialNumbers;
|
|
||||||
// for (int i = 0; i < num; i++)
|
|
||||||
// {
|
|
||||||
// std::string serialNumber = "";
|
|
||||||
// for (int j = 0; j < 20; j++) // 提取序列号
|
|
||||||
// {
|
|
||||||
// if (pinfo[i].str_Serial_Num[j] == '\0') // 结尾判断
|
|
||||||
// break;
|
|
||||||
// serialNumber += pinfo[i].str_Serial_Num[j];
|
|
||||||
// }
|
|
||||||
// serialNumbers.push_back(serialNumber);
|
|
||||||
// }
|
|
||||||
|
|
||||||
// // 按照字典序对序列号进行排序
|
|
||||||
// std::sort(serialNumbers.begin(), serialNumbers.end(), compare_serial_numbers);
|
|
||||||
|
|
||||||
// // 将排序后的序列号和索引进行绑定
|
|
||||||
// for (int i = 0; i < serialNumbers.size(); i++)
|
|
||||||
// {
|
|
||||||
// deviceMap[serialNumbers[i]] = i;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// return deviceMap; // 返回序列号与设备索引的映射关系
|
|
||||||
// }
|
|
||||||
|
|
||||||
// void operate_multiple_can_devices()
|
|
||||||
// {
|
|
||||||
// auto deviceMap = get_device_map(); // 获取设备序列号与设备索引的映射
|
|
||||||
|
|
||||||
// // 遍历所有设备
|
|
||||||
// for (const auto &pair : deviceMap)
|
|
||||||
// {
|
|
||||||
// const std::string &serialNumber = pair.first; // 设备序列号
|
|
||||||
// int nDeviceInd = pair.second; // 设备索引
|
|
||||||
// int nDeviceType = 4; // 根据实际设备类型设置
|
|
||||||
// DWORD dwRel;
|
|
||||||
// VCI_INIT_CONFIG vic;
|
|
||||||
|
|
||||||
// std::cout << "正在操作设备: " << serialNumber << " (索引: " << nDeviceInd << ")" << std::endl;
|
|
||||||
|
|
||||||
// // 打开设备
|
|
||||||
// dwRel = VCI_OpenDevice(nDeviceType, nDeviceInd, 0);
|
|
||||||
// if (dwRel != 1)
|
|
||||||
// {
|
|
||||||
// std::cout << "无法打开设备 " << serialNumber << std::endl;
|
|
||||||
// continue;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// // 初始化 CAN 通道
|
|
||||||
// vic.AccCode = 0x80000008;
|
|
||||||
// vic.AccMask = 0xFFFFFFFF;
|
|
||||||
// vic.Filter = 1;
|
|
||||||
// vic.Timing0 = 0x00;
|
|
||||||
// vic.Timing1 = 0x14;
|
|
||||||
// vic.Mode = 0;
|
|
||||||
|
|
||||||
// if (VCI_InitCAN(nDeviceType, nDeviceInd, 0, &vic) != 1)
|
|
||||||
// {
|
|
||||||
// std::cout << "初始化设备 " << serialNumber << " 的 CAN 通道 0 失败" << std::endl;
|
|
||||||
// VCI_CloseDevice(nDeviceType, nDeviceInd);
|
|
||||||
// continue;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// if (VCI_InitCAN(nDeviceType, nDeviceInd, 1, &vic) != 1)
|
|
||||||
// {
|
|
||||||
// std::cout << "初始化设备 " << serialNumber << " 的 CAN 通道 1 失败" << std::endl;
|
|
||||||
// VCI_CloseDevice(nDeviceType, nDeviceInd);
|
|
||||||
// continue;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// // 启动 CAN 通道
|
|
||||||
// if (VCI_StartCAN(nDeviceType, nDeviceInd, 0) != 1)
|
|
||||||
// {
|
|
||||||
// std::cout << "启动设备 " << serialNumber << " 的 CAN 通道 0 失败" << std::endl;
|
|
||||||
// VCI_CloseDevice(nDeviceType, nDeviceInd);
|
|
||||||
// continue;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// if (VCI_StartCAN(nDeviceType, nDeviceInd, 1) != 1)
|
|
||||||
// {
|
|
||||||
// std::cout << "启动设备 " << serialNumber << " 的 CAN 通道 1 失败" << std::endl;
|
|
||||||
// VCI_CloseDevice(nDeviceType, nDeviceInd);
|
|
||||||
// continue;
|
|
||||||
// }
|
|
||||||
|
|
||||||
// std::cout << "设备 " << serialNumber << " 的所有 CAN 通道启动成功!" << std::endl;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
@ -1,160 +0,0 @@
|
|||||||
#ifndef _TI5LOGIC_H_
|
|
||||||
#define _TI5LOGIC_H_
|
|
||||||
#include "mathfunc.h"
|
|
||||||
#include <iostream>
|
|
||||||
#include <time.h>
|
|
||||||
#include <cmath>
|
|
||||||
#include "tool.h"
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
extern "C"
|
|
||||||
{ // 添加extern "C"
|
|
||||||
class pos_trans
|
|
||||||
{
|
|
||||||
protected:
|
|
||||||
float j[7] = {0, 0, 0, 0, 0, 0, 0};
|
|
||||||
float orij[7];
|
|
||||||
float prcj = 0.017;
|
|
||||||
float scd = sin(prcj) * sin(prcj);
|
|
||||||
float prc1 = float(int(10000 * sin(prcj) + 1)) / 10000;
|
|
||||||
float prc2 = prc1 * (prc1 + 2);
|
|
||||||
float prc3 = prc1 * (3 + prc1 + prc1 * prc1) + prc2;
|
|
||||||
float P_0[19][3] = {{0, 0, 0}}; // 机械臂按线性顺序的点集
|
|
||||||
struct ROD
|
|
||||||
{
|
|
||||||
int i1, i2; // 连杆端点为P_0[i1]和P_0[i2]
|
|
||||||
float r; // 连杆半径
|
|
||||||
float l; // 连杆长度
|
|
||||||
};
|
|
||||||
ROD rod[18]; // 机械臂按线性顺序的连杆集
|
|
||||||
float len[18]; // 数学模型的长度参数（与rod.l独立）
|
|
||||||
int rodnum; // 连杆总数
|
|
||||||
// 检测两连杆是否碰撞，P1、P2构成一根连杆，半径为r1；P3、P4构成另一根连杆，半径为r2
|
|
||||||
bool LLcolsp(float P1[3], float P2[3], float P3[3], float P4[3], float r1, float r2);
|
|
||||||
void init_rodindex();
|
|
||||||
// 若点距和连杆长度一致则认为点坐标值正确
|
|
||||||
bool check_Points();
|
|
||||||
// 根据变换矩阵计算位姿
|
|
||||||
void MatrixT2ypr(float T[4][4]);
|
|
||||||
// 根据位姿计算变换矩阵
|
|
||||||
void Pos2MatrixT(float T[4][4]);
|
|
||||||
// 修正角度并检测关节范围是否合法（相邻连杆间是否碰撞）
|
|
||||||
bool mendjoints();
|
|
||||||
bool checkcalj();
|
|
||||||
// 检测所有连杆间的碰撞
|
|
||||||
bool check_colsp();
|
|
||||||
|
|
||||||
public:
|
|
||||||
float jr1[7] = {-pi, -pi, -pi, -pi, -pi, -pi, -pi}; // 关节静态角度范围
|
|
||||||
float jr2[7] = {pi, pi, pi, pi, pi, pi, pi};
|
|
||||||
float gap0, gap;
|
|
||||||
float ypr[6] = {0, 0, 0};
|
|
||||||
// float j[7] = {0, 0, 0, 0, 0, 0, 0};
|
|
||||||
void showpointsinfo();
|
|
||||||
};
|
|
||||||
|
|
||||||
class humanoidLeftArm : public pos_trans
|
|
||||||
{
|
|
||||||
protected:
|
|
||||||
// 初始化固有的机械结构
|
|
||||||
void init_arm_structure()
|
|
||||||
{
|
|
||||||
rodnum = 3;
|
|
||||||
rod[0].r = 10, rod[1].r = 10, rod[2].r = 10; // 连杆半径 170A
|
|
||||||
rod[0].l = 171, rod[1].l = 250, rod[2].l = 250; // 连杆长度 170A
|
|
||||||
// rod[0].r = 45, rod[1].r = 42, rod[2].r = 37.5; // 连杆半径 T230
|
|
||||||
// rod[0].l = 234.5, rod[1].l = 339, rod[2].l = 301.5; // 连杆长度 T230
|
|
||||||
init_rodindex();
|
|
||||||
}
|
|
||||||
// 初始化数学模型参数
|
|
||||||
void init_model_structure()
|
|
||||||
{
|
|
||||||
float M = 1.57;
|
|
||||||
jr1[6] = -pi/6;
|
|
||||||
// jr1[0] = -M, jr1[1] = -M, jr1[2] = -0.5, jr1[3] = -M, jr1[4] = -M, jr1[5] = -M, jr1[6] = -pi/6;
|
|
||||||
// jr2[0] = M, jr2[1] = M, jr2[2] = 0.5, jr2[3] = 0, jr2[4] = M, jr2[5] = M, jr2[6] = M;
|
|
||||||
gap = abs(jr1[0]);
|
|
||||||
for (int i = 1; i < 7; i++)
|
|
||||||
if (abs(jr1[i]) > gap)
|
|
||||||
gap = abs(jr1[i]);
|
|
||||||
for (int i = 0; i < 7; i++)
|
|
||||||
if (abs(jr2[i]) > gap)
|
|
||||||
gap = abs(jr2[i]);
|
|
||||||
gap0 = gap;
|
|
||||||
len[0] = rod[0].l, len[1] = rod[1].l, len[2] = rod[2].l;
|
|
||||||
P_0[1][0] = 0, P_0[1][1] = len[0], P_0[1][2] = 0;
|
|
||||||
float goal_j[7] = {0, 0, 0, 0, 0, 0, 0};
|
|
||||||
l_forward_move(goal_j); // show_value("pos",pos,6);
|
|
||||||
ypr[0]=ypr[2]=0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 点或向量在不同坐标系转换（p=true表示点，p=false表示向量）
|
|
||||||
void fromS0toS1(float P0[3], float P[3], bool p);
|
|
||||||
void fromS1toS2(float P0[3], float P[3], bool p);
|
|
||||||
void fromS2toS3(float P0[3], float P[3], bool p);
|
|
||||||
void fromS3toS4(float P0[3], float P[3], bool p);
|
|
||||||
void fromS4toS5(float P0[3], float P[3], bool p);
|
|
||||||
|
|
||||||
void fromS5toS6(float P0[3], float P[3], bool p);
|
|
||||||
void fromS6toS7(float P0[3], float P[3], bool p);
|
|
||||||
|
|
||||||
void fromS1toS0(float P0[3], float P[3], bool p);
|
|
||||||
void fromS2toS1(float P0[3], float P[3], bool p);
|
|
||||||
void fromS3toS2(float P0[3], float P[3], bool p);
|
|
||||||
void fromS4toS3(float P0[3], float P[3], bool p);
|
|
||||||
void fromS5toS4(float P0[3], float P[3], bool p);
|
|
||||||
void fromS6toS5(float P0[3], float P[3], bool p);
|
|
||||||
void fromS7toS6(float P0[3], float P[3], bool p);
|
|
||||||
bool solve_in_S6();
|
|
||||||
bool solve_in_S4();
|
|
||||||
bool solve_in_S2();
|
|
||||||
// 逆运动解算器
|
|
||||||
bool Points2J();
|
|
||||||
// 根据角度计算点变换矩阵
|
|
||||||
void J2MatrixT(float T[4][4]);
|
|
||||||
|
|
||||||
public:
|
|
||||||
humanoidLeftArm();
|
|
||||||
// 检测逆运动解出的角是否满足原始位姿
|
|
||||||
bool checkacc();
|
|
||||||
// 正运动
|
|
||||||
bool l_forward_move(float goal_j[7]);
|
|
||||||
|
|
||||||
// 获取当前角度
|
|
||||||
void get_crt_j(float joints[7]);
|
|
||||||
|
|
||||||
// 获取当前位置
|
|
||||||
void get_crt_pos(float pos[6]);
|
|
||||||
|
|
||||||
/*
|
|
||||||
参数：
|
|
||||||
pos:目标位置
|
|
||||||
value：dim的值
|
|
||||||
dim：-1~2 代表x,y,z，-1的时候是没有臂角约束，只会接收末端位姿，其他参数忽略
|
|
||||||
absolute：true的时候是绝对位置（以胸部原点位置），false的时候是相对位置（以当前点胳膊轴位置）
|
|
||||||
*/
|
|
||||||
// bool l_backward_move(float pos[6], float value, int dim, bool absolute);
|
|
||||||
bool l_backward_move(float pos[6]);
|
|
||||||
// 测试fromSi2Sj函数是否正确（是否抄错）
|
|
||||||
void testj2p();
|
|
||||||
void show_crt_pos();
|
|
||||||
void show_crt_j();
|
|
||||||
};
|
|
||||||
class humanoidRightArm : public humanoidLeftArm
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
void get_r_ypr(float r_ypr[3]);
|
|
||||||
public:
|
|
||||||
humanoidRightArm();
|
|
||||||
bool r_forward_move(float goal_j[7]);
|
|
||||||
void get_crt_j(float joints[7]);
|
|
||||||
void get_crt_pos(float postrue[6]);
|
|
||||||
// bool r_backward_move(float postrue[6], float value, int dim, bool absolute);
|
|
||||||
bool r_backward_move(float postrue[6]);
|
|
||||||
void show_crt_pos();
|
|
||||||
void show_crt_j();
|
|
||||||
void showpointsinfo();
|
|
||||||
};
|
|
||||||
|
|
||||||
} // 添加extern "C"
|
|
||||||
#endif
|
|
||||||
@ -1,46 +0,0 @@
|
|||||||
#ifndef TI5MOVE_H
|
|
||||||
#define TI5MOVE_H
|
|
||||||
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <cstdlib>
|
|
||||||
#include <chrono>
|
|
||||||
#include <thread>
|
|
||||||
// #include "can/SingleCaninterface.h"
|
|
||||||
// #include "can/motortypehelper.h"
|
|
||||||
#include <vector>
|
|
||||||
#include "tool.h"
|
|
||||||
#include "Ti5LOGIC.h"
|
|
||||||
#include <time.h>
|
|
||||||
#include <mutex>
|
|
||||||
#include "Ti5CAN_Driver.h"
|
|
||||||
|
|
||||||
#define USLEEPTIME 3000
|
|
||||||
|
|
||||||
extern "C"{ //添加：extern C
|
|
||||||
class ArmController{
|
|
||||||
private:
|
|
||||||
float AG = 0.005; //启停时变速的采样间距（秒
|
|
||||||
float scale = 101; //电机内圈与外圈的速度比
|
|
||||||
float n2p = 655.36; //内圈转速到步速的转化系数
|
|
||||||
// float n2p = 655.36/101; //双编码器的 内圈转速到步速的转化系数
|
|
||||||
float mvtime = 0;
|
|
||||||
// float j2p = scale * 65536 / 2 / pi; //电机外圈角度到内圈步数的转化
|
|
||||||
float min_time = 0;
|
|
||||||
bool jstp = false;
|
|
||||||
uint8_t canidList[IDNUM];
|
|
||||||
float nplL[4][4]; //add 用于linear_move函数机械臂直线运动
|
|
||||||
void setn(int npL[IDNUM],int deviceInd,int canInd);
|
|
||||||
void ACTmove(float *a,float *b,float T0,int deviceInd,int canInd);// 实际运动
|
|
||||||
public:
|
|
||||||
ArmController(uint8_t canid[IDNUM]);
|
|
||||||
// void plan_move(float crtj[IDNUM],int canInd,int deviceInd);
|
|
||||||
void plan_move(int deviceInd, int canInd,float crtj[IDNUM]);
|
|
||||||
void new_plan_move(int deviceInd,int canInd,float crtj[IDNUM]);//mfs add 2024-12-3
|
|
||||||
void GETP_plan_move(float crtj[IDNUM],float *CUrrentJointPosition,int deviceInd,int canInd);
|
|
||||||
float NMAX = 3000; //所有电机内核最大转速值（(NMAX/100)圈/秒）
|
|
||||||
float j2p = scale * 65536 / 2 / pi; //电机外圈角度到内圈步数的转化
|
|
||||||
// float j2p = 131072/pi; //双编码器 电机外圈角度到内圈步数的转化
|
|
||||||
};
|
|
||||||
}//添加：extern C
|
|
||||||
|
|
||||||
#endif
|
|
||||||
@ -1,104 +0,0 @@
|
|||||||
#ifndef CONTROLCAN_H
|
|
||||||
#define CONTROLCAN_H
|
|
||||||
|
|
||||||
////文件版本：v2.02 20190609
|
|
||||||
//接口卡类型定义
|
|
||||||
|
|
||||||
#define VCI_USBCAN1 3
|
|
||||||
#define VCI_USBCAN2 4
|
|
||||||
#define VCI_USBCAN2A 4
|
|
||||||
|
|
||||||
#define VCI_USBCAN_E_U 20
|
|
||||||
#define VCI_USBCAN_2E_U 21
|
|
||||||
|
|
||||||
//函数调用返回状态值
|
|
||||||
#define STATUS_OK 1
|
|
||||||
#define STATUS_ERR 0
|
|
||||||
|
|
||||||
#define USHORT unsigned short int
|
|
||||||
#define BYTE unsigned char
|
|
||||||
#define CHAR char
|
|
||||||
#define UCHAR unsigned char
|
|
||||||
#define UINT unsigned int
|
|
||||||
#define DWORD unsigned int
|
|
||||||
#define PVOID void*
|
|
||||||
#define ULONG unsigned int
|
|
||||||
#define INT int
|
|
||||||
#define UINT32 UINT
|
|
||||||
#define LPVOID void*
|
|
||||||
#define BOOL BYTE
|
|
||||||
#define TRUE 1
|
|
||||||
#define FALSE 0
|
|
||||||
|
|
||||||
//1.ZLGCAN系列接口卡信息的数据类型。
|
|
||||||
typedef struct _VCI_BOARD_INFO{
|
|
||||||
USHORT hw_Version;
|
|
||||||
USHORT fw_Version;
|
|
||||||
USHORT dr_Version;
|
|
||||||
USHORT in_Version;
|
|
||||||
USHORT irq_Num;
|
|
||||||
BYTE can_Num;
|
|
||||||
CHAR str_Serial_Num[20];
|
|
||||||
CHAR str_hw_Type[40];
|
|
||||||
USHORT Reserved[4];
|
|
||||||
} VCI_BOARD_INFO,*PVCI_BOARD_INFO;
|
|
||||||
|
|
||||||
//2.定义CAN信息帧的数据类型。
|
|
||||||
typedef struct _VCI_CAN_OBJ{
|
|
||||||
UINT ID;
|
|
||||||
UINT TimeStamp;
|
|
||||||
BYTE TimeFlag;
|
|
||||||
BYTE SendType;
|
|
||||||
BYTE RemoteFlag;//是否是远程帧
|
|
||||||
BYTE ExternFlag;//是否是扩展帧
|
|
||||||
BYTE DataLen;
|
|
||||||
BYTE Data[8];
|
|
||||||
BYTE Reserved[3];
|
|
||||||
}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
|
|
||||||
|
|
||||||
//3.定义初始化CAN的数据类型
|
|
||||||
typedef struct _INIT_CONFIG{
|
|
||||||
DWORD AccCode;
|
|
||||||
DWORD AccMask;
|
|
||||||
DWORD Reserved;
|
|
||||||
UCHAR Filter;
|
|
||||||
UCHAR Timing0;
|
|
||||||
UCHAR Timing1;
|
|
||||||
UCHAR Mode;
|
|
||||||
}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
|
|
||||||
|
|
||||||
///////// new add struct for filter /////////
|
|
||||||
typedef struct _VCI_FILTER_RECORD{
|
|
||||||
DWORD ExtFrame; //是否为扩展帧
|
|
||||||
DWORD Start;
|
|
||||||
DWORD End;
|
|
||||||
}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD;
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
#define EXTERN_C extern "C"
|
|
||||||
#else
|
|
||||||
#define EXTERN_C
|
|
||||||
#endif
|
|
||||||
|
|
||||||
EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
|
|
||||||
EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
|
|
||||||
EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
|
|
||||||
|
|
||||||
EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
|
|
||||||
|
|
||||||
EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
|
|
||||||
|
|
||||||
EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
|
|
||||||
EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
|
|
||||||
|
|
||||||
EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
|
|
||||||
EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
|
|
||||||
|
|
||||||
EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,ULONG Len);
|
|
||||||
EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,ULONG Len,INT WaitTime);
|
|
||||||
|
|
||||||
EXTERN_C DWORD VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved);
|
|
||||||
EXTERN_C DWORD VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
@ -1,28 +0,0 @@
|
|||||||
#ifndef MATHFUNC_H
|
|
||||||
#define MATHFUNC_H
|
|
||||||
|
|
||||||
#include<math.h>
|
|
||||||
#define pi M_PI
|
|
||||||
|
|
||||||
extern "C"{//添加extern "C"
|
|
||||||
float vec_dot(float v1[3],float v2[3]);
|
|
||||||
float vec_length(float v[3]);
|
|
||||||
void vec_rescale(float v[3],float l,float nv[3]);
|
|
||||||
int fx2solv(float a,float b,float c,float *p1,float *p2);
|
|
||||||
float pointsdistance(float P1[3],float P2[3]);
|
|
||||||
void vec_subtraction(float v1[3],float v2[3],float v[3]);
|
|
||||||
void vec_addition(float v1[3],float v2[3],float v[3]);
|
|
||||||
|
|
||||||
float vec_angle(float v1[3],float v2[3],float f);
|
|
||||||
float solve_trangle(float a,float b,float c);
|
|
||||||
void vec_cross(float v1[3],float v2[3],float v[3]);
|
|
||||||
float calcu_angle(float sa,float ca,float cp2);
|
|
||||||
|
|
||||||
|
|
||||||
void MatrixmultPoint(float T[4][4],float P0[3],float P[3]);
|
|
||||||
void Matrixmult(float T1[4][4],float T2[4][4],float T[4][4]);
|
|
||||||
void MatrixT2MatrixI(float T[4][4],float I[4][4]);
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
@ -1,35 +0,0 @@
|
|||||||
#ifndef TOOL_H
|
|
||||||
#define TOOL_H
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <string.h>
|
|
||||||
#include <termio.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
#define IDNUM 7
|
|
||||||
|
|
||||||
|
|
||||||
// extern "C"
|
|
||||||
// { // 添加extern "C"
|
|
||||||
void Mcopy(float (*C)[4], float (*P)[4]);
|
|
||||||
|
|
||||||
void show_value(string name, float (*T)[4]);
|
|
||||||
void copy_value(float *copy, float *paste, int n);
|
|
||||||
void show_value(string name, float *value);
|
|
||||||
void show_value(string name,float* value,int n);
|
|
||||||
void show_value(string name, uint32_t *value);
|
|
||||||
void show_point(string name, float *value);
|
|
||||||
void show_value(string name, uint8_t *value);
|
|
||||||
void get_cmdlist(uint8_t *L, uint8_t c);
|
|
||||||
void get_paralist(uint32_t *L, uint32_t c);
|
|
||||||
|
|
||||||
int getch();
|
|
||||||
char scanKeyboard();
|
|
||||||
|
|
||||||
// void test_demo();
|
|
||||||
// void test();
|
|
||||||
|
|
||||||
// } // 添加extern "C"
|
|
||||||
#endif
|
|
||||||
Loading…
Reference in New Issue
Block a user