SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update robot controller

Browse Source
This commit is contained in:
linbo 2025-10-24 17:06:17 +08:00
parent 0720ec8171
commit 505b373368
31 changed files with 15 additions and 2127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/hardware/can/motor_protocol/abstractmotorprotocol.h
View File

@ -16,9 +16,9 @@ namespace cmvr::hardware
cfg_ = cfg;
id_ = cfg.getAttrString("id");
joint_name_ = cfg.getAttrString("joint_name");
limitQLb_ = cfg.getAttrDefault("limitQLb_",(float)3.14);
limitQUb_ = cfg.getAttrDefault("limitQUb_",(float)3.14);
limitQd = cfg.getAttrDefault("limitQd",(float)3.0);
limitQLb_ = cfg.getAttrDefault("limitQLb_",3.14f);
limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
limitQd = cfg.getAttrDefault("limitQd",3.0f);
}
~AbstractMotorProtocol() = default;

2
src/devices/robot/CMakeLists.txt
View File

@ -1,4 +1,2 @@
#add_subdirectory(ti5_robot)
add_subdirectory(humanoid_robot)
#add_subdirectory(c701)
add_subdirectory(controller)

54
src/devices/robot/c701/CMakeLists.txt
View File

@ -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
)

400
src/devices/robot/c701/motor/motor_controller.cpp
View File

@ -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));
}

101
src/devices/robot/c701/motor/motor_controller.h
View File

@ -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);
};
}
}
}

86
src/devices/robot/c701/motor/motor_controller_test.cpp
View File

@ -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);
}

27
src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp
View File

@ -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;
}

60
src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h
View File

@ -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};
};
}
}
}

39
src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp
View File

@ -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();
}

26
src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h
View File

@ -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;
};
}
}
}

36
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp
View File

@ -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 ;
}
}

32
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h
View File

@ -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};
};
}
}
}

26
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp
View File

@ -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();
}

32
src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h
View File

@ -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};
};
}
}
}

4
src/devices/robot/controller/abstractcontroller.h
View File

@ -8,7 +8,7 @@
#include <curl/curl.h>
#include "rapidxml/xml_parser.h"
#include "jsoncpp/json/json.h"
namespace cmvr::device
{
enum ControllerState
@ -25,7 +25,7 @@ namespace cmvr::device
[[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;

2
src/devices/robot/controller/cartesiancontroller.cpp
View File

@ -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)
return;

3
src/devices/robot/controller/cartesiancontroller.h
View File

@ -12,9 +12,10 @@ namespace cmvr::device
{
public:
explicit CartesianController(const XmlNode& cfg);
void call(std::string json) override;
void call(const Json::Value& json) override;
void stop() override;
private:
};
}

7
src/devices/robot/controller/controller_manager.cpp
View File

@ -28,9 +28,9 @@ void ControllerManager::create(const XmlNode& cfg)
{
ComponentGroup group;
group.id = child.getAttrString("id");
// CanGroupID可能记录了多个cangroupID
std::string cangroupIDs = child.getAttrString("CanGroupID");
std::istringstream iss(cangroupIDs);
// CanGroupID可能记录了多个canGroupID
std::string canGroupIDs = child.getAttrString("CanGroupID");
std::istringstream iss(canGroupIDs);
std::string id;
while (std::getline(iss, 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);
}
}
//
componentGroups_[group.id] = group;
}

2
src/devices/robot/controller/controller_manager.h
View File

@ -37,7 +37,7 @@ namespace cmvr::device
// 清除错误信息
void clearError();
// 根据xml内容创建控制器组
void create(const XmlNode& cfg);
private:

2
src/devices/robot/controller/jointpositioncontroller.cpp
View File

@ -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)
return;

2
src/devices/robot/controller/jointpositioncontroller.h
View File

@ -12,7 +12,7 @@ namespace cmvr::device
{
public:
explicit JointPositionController(const XmlNode& cfg);
void call(std::string json) override;
void call(const Json::Value& json) override;
void stop() override;
private:
};

5
src/devices/robot/ti5_robot/CMakeLists.txt
View File

@ -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)

343
src/devices/robot/ti5_robot/ti5_robot.cpp
View File

@ -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;
//}

49
src/devices/robot/ti5_robot/ti5_robot.h
View File

@ -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

252
src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h
View File

@ -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

171
src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h
View File

@ -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;
// }
// }

160
src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h
View File

@ -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

46
src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h
View File

@ -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

104
src/devices/robot/ti5_robot/ti5include/controlcan.h
View File

@ -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

28
src/devices/robot/ti5_robot/ti5include/mathfunc.h
View File

@ -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

35
src/devices/robot/ti5_robot/ti5include/tool.h
View File

@ -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