From 505b373368fc6722449b4637437ae0e83170fd50 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 24 Oct 2025 17:06:17 +0800 Subject: [PATCH] update robot controller --- .../motor_protocol/abstractmotorprotocol.h | 6 +- src/devices/robot/CMakeLists.txt | 2 - src/devices/robot/c701/CMakeLists.txt | 54 --- .../robot/c701/motor/motor_controller.cpp | 400 ------------------ .../robot/c701/motor/motor_controller.h | 101 ----- .../c701/motor/motor_controller_test.cpp | 86 ---- .../c701/motor/protocol/ti5_motor_rpdo1.cpp | 27 -- .../c701/motor/protocol/ti5_motor_rpdo1.h | 60 --- .../motor/protocol/ti5_motor_sdo_response.cpp | 39 -- .../motor/protocol/ti5_motor_sdo_response.h | 26 -- .../c701/motor/protocol/ti5_motor_tpdo1.cpp | 36 -- .../c701/motor/protocol/ti5_motor_tpdo1.h | 32 -- .../c701/motor/protocol/ti5_motor_tpdo2.cpp | 26 -- .../c701/motor/protocol/ti5_motor_tpdo2.h | 32 -- .../robot/controller/abstractcontroller.h | 4 +- .../robot/controller/cartesiancontroller.cpp | 2 +- .../robot/controller/cartesiancontroller.h | 3 +- .../robot/controller/controller_manager.cpp | 7 +- .../robot/controller/controller_manager.h | 2 +- .../controller/jointpositioncontroller.cpp | 2 +- .../controller/jointpositioncontroller.h | 2 +- src/devices/robot/ti5_robot/CMakeLists.txt | 5 - src/devices/robot/ti5_robot/ti5_robot.cpp | 343 --------------- src/devices/robot/ti5_robot/ti5_robot.h | 49 --- .../robot/ti5_robot/ti5include/Ti5BASIC.h | 252 ----------- .../ti5_robot/ti5include/Ti5CAN_Driver.h | 171 -------- .../robot/ti5_robot/ti5include/Ti5LOGIC.h | 160 ------- .../robot/ti5_robot/ti5include/Ti5MOVE.h | 46 -- .../robot/ti5_robot/ti5include/controlcan.h | 104 ----- .../robot/ti5_robot/ti5include/mathfunc.h | 28 -- src/devices/robot/ti5_robot/ti5include/tool.h | 35 -- 31 files changed, 15 insertions(+), 2127 deletions(-) delete mode 100644 src/devices/robot/c701/CMakeLists.txt delete mode 100644 src/devices/robot/c701/motor/motor_controller.cpp delete mode 100644 src/devices/robot/c701/motor/motor_controller.h delete mode 100644 src/devices/robot/c701/motor/motor_controller_test.cpp delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp delete mode 100644 src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h delete mode 100644 src/devices/robot/ti5_robot/CMakeLists.txt delete mode 100644 src/devices/robot/ti5_robot/ti5_robot.cpp delete mode 100644 src/devices/robot/ti5_robot/ti5_robot.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/controlcan.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/mathfunc.h delete mode 100644 src/devices/robot/ti5_robot/ti5include/tool.h diff --git a/include/hardware/can/motor_protocol/abstractmotorprotocol.h b/include/hardware/can/motor_protocol/abstractmotorprotocol.h index 81e88af8..15e89f78 100644 --- a/include/hardware/can/motor_protocol/abstractmotorprotocol.h +++ b/include/hardware/can/motor_protocol/abstractmotorprotocol.h @@ -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; diff --git a/src/devices/robot/CMakeLists.txt b/src/devices/robot/CMakeLists.txt index 3280264f..0f9a8303 100644 --- a/src/devices/robot/CMakeLists.txt +++ b/src/devices/robot/CMakeLists.txt @@ -1,4 +1,2 @@ -#add_subdirectory(ti5_robot) add_subdirectory(humanoid_robot) -#add_subdirectory(c701) add_subdirectory(controller) \ No newline at end of file diff --git a/src/devices/robot/c701/CMakeLists.txt b/src/devices/robot/c701/CMakeLists.txt deleted file mode 100644 index aae00daa..00000000 --- a/src/devices/robot/c701/CMakeLists.txt +++ /dev/null @@ -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 -) - - diff --git a/src/devices/robot/c701/motor/motor_controller.cpp b/src/devices/robot/c701/motor/motor_controller.cpp deleted file mode 100644 index 5586d6fc..00000000 --- a/src/devices/robot/c701/motor/motor_controller.cpp +++ /dev/null @@ -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 >(); - - // NMT - message_manager_->AddSendProtocolData, false>(); - - //sync - message_manager_->AddSendProtocolData, false>(); - for (const auto node_id: node_ids_) { - //nmt - message_manager_->AddRecvProtocolData, false>(node_id); - - //sdo - message_manager_->AddSendProtocolData, false>(node_id); - message_manager_->AddRecvProtocolData(node_id); - - //TPDO - message_manager_->AddRecvProtocolData(node_id); - message_manager_->AddRecvProtocolData(node_id); - - //RPDO - message_manager_->AddSendProtocolData(node_id); - } - - // 初始化 sender - can_sender_ = std::make_shared >(); - 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 >(); - 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 *>( - message_manager_->GetMutableProtocolDataById(NmtRequestProtocol::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 *>( - message_manager_->GetMutableProtocolDataById(SyncProtocol::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 *>( - message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::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( - 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(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(cmd)); - break; - case RUN_MODE_PROFILE_POSITION: - SetPPTargetPosByPdo(node_id,static_cast(cmd)); - // SetPPTargetPosBySdo(node_id,static_cast(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)); -} - - - diff --git a/src/devices/robot/c701/motor/motor_controller.h b/src/devices/robot/c701/motor/motor_controller.h deleted file mode 100644 index ba8551be..00000000 --- a/src/devices/robot/c701/motor/motor_controller.h +++ /dev/null @@ -1,101 +0,0 @@ -// -// Created by lgv on 2025/7/17. -// - -#pragma once - -#include - -#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 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 GetRobotDetail() { - auto data_ptr = std::make_unique(); - 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 node_ids_{}; // 要控制的电机 id - // std::map control_commands_{}; - - - // nmt - device::NmtRequestProtocol* nmt_command_{nullptr}; - - //sync - device::SyncProtocol* sync_command_{nullptr}; - - // sdo - std::map*> sdo_commands_{}; - - // rpdo1 - std::map rpdo1_commands_{}; - - - std::shared_ptr> can_receiver_{nullptr}; - std::shared_ptr> can_sender_{nullptr}; - std::shared_ptr> 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); - - }; - } - } -} diff --git a/src/devices/robot/c701/motor/motor_controller_test.cpp b/src/devices/robot/c701/motor/motor_controller_test.cpp deleted file mode 100644 index 3e695e17..00000000 --- a/src/devices/robot/c701/motor/motor_controller_test.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// -// Created by lgv on 2025/7/18. -// - - -#include - -#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 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); -} \ No newline at end of file diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp b/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp deleted file mode 100644 index 89b3f5c1..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp +++ /dev/null @@ -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 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; -} diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h b/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h deleted file mode 100644 index 770dc77b..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h +++ /dev/null @@ -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 - -namespace cmvr { - namespace robot { - namespace motor { - class Ti5MotorRPDO1 : public device::ProtocolData { - 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 lock(mutex_); - target_pos_ = position; - } - - void SetTargetVel(int32_t velocity) { - std::lock_guard lock(mutex_); - target_vel_ = velocity; - } - - void SetCtrlWord(uint16_t ctrl_word) { - std::lock_guard 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}; - }; - } - } -} \ No newline at end of file diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp b/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp deleted file mode 100644 index d1fbc3dd..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp +++ /dev/null @@ -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(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(); -} diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h b/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h deleted file mode 100644 index a19b3773..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h +++ /dev/null @@ -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 { - public: - explicit Ti5MotorSdoResponse(uint8_t node_id) - : SdoResponseProtocol(node_id) { - } - - protected: - void ParseSdoData(const msgs::SdoFrame &sdo_response, - cmvr::msgs::RobotDetail *sensor_data) const override; - }; - } - } -} diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp b/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp deleted file mode 100644 index f053e4e4..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp +++ /dev/null @@ -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(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 ; - } - - - -} diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h b/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h deleted file mode 100644 index c8e68471..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h +++ /dev/null @@ -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 { - 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}; - }; - } - } -} \ No newline at end of file diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp b/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp deleted file mode 100644 index 50798341..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp +++ /dev/null @@ -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(); -} diff --git a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h b/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h deleted file mode 100644 index 9eee93d2..00000000 --- a/src/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h +++ /dev/null @@ -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 { - 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}; - }; - } - } -} \ No newline at end of file diff --git a/src/devices/robot/controller/abstractcontroller.h b/src/devices/robot/controller/abstractcontroller.h index 56037883..6e312c2d 100644 --- a/src/devices/robot/controller/abstractcontroller.h +++ b/src/devices/robot/controller/abstractcontroller.h @@ -8,7 +8,7 @@ #include #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; diff --git a/src/devices/robot/controller/cartesiancontroller.cpp b/src/devices/robot/controller/cartesiancontroller.cpp index aa53c7c5..a6bd9b6e 100644 --- a/src/devices/robot/controller/cartesiancontroller.cpp +++ b/src/devices/robot/controller/cartesiancontroller.cpp @@ -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; diff --git a/src/devices/robot/controller/cartesiancontroller.h b/src/devices/robot/controller/cartesiancontroller.h index 328f8ab2..3cd615f7 100644 --- a/src/devices/robot/controller/cartesiancontroller.h +++ b/src/devices/robot/controller/cartesiancontroller.h @@ -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: + }; } diff --git a/src/devices/robot/controller/controller_manager.cpp b/src/devices/robot/controller/controller_manager.cpp index b2872471..0a21b1a2 100644 --- a/src/devices/robot/controller/controller_manager.cpp +++ b/src/devices/robot/controller/controller_manager.cpp @@ -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(controller_node); } } + // componentGroups_[group.id] = group; } diff --git a/src/devices/robot/controller/controller_manager.h b/src/devices/robot/controller/controller_manager.h index facc6ae6..c6262a22 100644 --- a/src/devices/robot/controller/controller_manager.h +++ b/src/devices/robot/controller/controller_manager.h @@ -37,7 +37,7 @@ namespace cmvr::device // 清除错误信息 void clearError(); - + // 根据xml内容创建控制器组 void create(const XmlNode& cfg); private: diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index 45947f3f..cc68e132 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -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; diff --git a/src/devices/robot/controller/jointpositioncontroller.h b/src/devices/robot/controller/jointpositioncontroller.h index 8a2c28c0..5e37eb2b 100644 --- a/src/devices/robot/controller/jointpositioncontroller.h +++ b/src/devices/robot/controller/jointpositioncontroller.h @@ -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: }; diff --git a/src/devices/robot/ti5_robot/CMakeLists.txt b/src/devices/robot/ti5_robot/CMakeLists.txt deleted file mode 100644 index fbbb4ab0..00000000 --- a/src/devices/robot/ti5_robot/CMakeLists.txt +++ /dev/null @@ -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) \ No newline at end of file diff --git a/src/devices/robot/ti5_robot/ti5_robot.cpp b/src/devices/robot/ti5_robot/ti5_robot.cpp deleted file mode 100644 index 704da045..00000000 --- a/src/devices/robot/ti5_robot/ti5_robot.cpp +++ /dev/null @@ -1,343 +0,0 @@ -//#include "ti5_robot.h" -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -// -//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 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 &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(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(pose.position.x), static_cast(pose.position.y), static_cast(pose.position.z), -// static_cast(pose.euler.rx), static_cast(pose.euler.ry), static_cast(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> &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 &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 &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 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 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; -//} diff --git a/src/devices/robot/ti5_robot/ti5_robot.h b/src/devices/robot/ti5_robot/ti5_robot.h deleted file mode 100644 index 70e5f7b6..00000000 --- a/src/devices/robot/ti5_robot/ti5_robot.h +++ /dev/null @@ -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 &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> &traj, double dt) override; - void followPoseTrajectory(std::vector &traj, double dt) override; - void servoJ(std::vector &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 \ No newline at end of file diff --git a/src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h b/src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h deleted file mode 100644 index 382a3cf4..00000000 --- a/src/devices/robot/ti5_robot/ti5include/Ti5BASIC.h +++ /dev/null @@ -1,252 +0,0 @@ -#ifndef Ti5BASIC_H -#define Ti5BASIC_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 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 \ No newline at end of file diff --git a/src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h b/src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h deleted file mode 100644 index 05a8e7ff..00000000 --- a/src/devices/robot/ti5_robot/ti5include/Ti5CAN_Driver.h +++ /dev/null @@ -1,171 +0,0 @@ -#ifndef TI5CAN_DRIVER_H -#define TI5CAN_DRIVER_H - -#include -#include -#include -#include -#include -#include -#include - - -#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 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 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 get_device_map() -// { -// VCI_BOARD_INFO pinfo[50]; -// std::unordered_map deviceMap; -// int num = VCI_FindUsbDevice2(pinfo); // 查询所有CAN设备 - -// // 提取所有设备的序列号 -// std::vector 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; -// } -// } diff --git a/src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h b/src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h deleted file mode 100644 index 91125a4d..00000000 --- a/src/devices/robot/ti5_robot/ti5include/Ti5LOGIC.h +++ /dev/null @@ -1,160 +0,0 @@ -#ifndef _TI5LOGIC_H_ -#define _TI5LOGIC_H_ -#include "mathfunc.h" -#include -#include -#include -#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 diff --git a/src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h b/src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h deleted file mode 100644 index 3b4171d0..00000000 --- a/src/devices/robot/ti5_robot/ti5include/Ti5MOVE.h +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef TI5MOVE_H -#define TI5MOVE_H - -#include -#include -#include -#include -// #include "can/SingleCaninterface.h" -// #include "can/motortypehelper.h" -#include -#include "tool.h" -#include "Ti5LOGIC.h" -#include -#include -#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 \ No newline at end of file diff --git a/src/devices/robot/ti5_robot/ti5include/controlcan.h b/src/devices/robot/ti5_robot/ti5include/controlcan.h deleted file mode 100644 index d0f99994..00000000 --- a/src/devices/robot/ti5_robot/ti5include/controlcan.h +++ /dev/null @@ -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 - diff --git a/src/devices/robot/ti5_robot/ti5include/mathfunc.h b/src/devices/robot/ti5_robot/ti5include/mathfunc.h deleted file mode 100644 index 88c401db..00000000 --- a/src/devices/robot/ti5_robot/ti5include/mathfunc.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef MATHFUNC_H -#define MATHFUNC_H - -#include -#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 \ No newline at end of file diff --git a/src/devices/robot/ti5_robot/ti5include/tool.h b/src/devices/robot/ti5_robot/ti5include/tool.h deleted file mode 100644 index 99399437..00000000 --- a/src/devices/robot/ti5_robot/ti5include/tool.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef TOOL_H -#define TOOL_H - -#include -#include -#include -#include - -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