113 lines
3.3 KiB
C++
113 lines
3.3 KiB
C++
//
|
|
// Created by lgv on 2025/8/1.
|
|
//
|
|
|
|
#include "motor/motor_manager.h"
|
|
#include "gtest/gtest.h"
|
|
#include "motor/ti5_motor/ti5_motor.h"
|
|
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
|
|
|
|
|
#include "canbus/can_comm/can_sender.h"
|
|
#include "canbus/can_comm/message_manager.h"
|
|
#include "canbus/can_client/socket/socket_can_client_raw.h"
|
|
#include "canbus/can_client/pcan//pcan_client.h"
|
|
|
|
using namespace cmvr::device;
|
|
using namespace cmvr::msgs;
|
|
|
|
TEST(MotorMangerTest,MyTest) {
|
|
|
|
uint8_t id = 22;
|
|
XmlNode cfg;
|
|
|
|
// 1 === 初始化公共组件 ===
|
|
auto can_client = std::make_shared<SocketCanClientRaw>(cfg);
|
|
auto sender = std::make_shared<CanSender<RobotDetail>>();
|
|
auto receiver = std::make_shared<CanReceiver<RobotDetail>>();
|
|
auto message_manager = std::make_shared<MessageManager<RobotDetail>>();
|
|
|
|
|
|
can_client->init();
|
|
auto ret = sender->Init(can_client.get(), false);
|
|
if (ret != ErrorCode::OK) {
|
|
LOG(ERROR) << "Failed to init can sender.";
|
|
}
|
|
|
|
|
|
ret = receiver->Init(can_client.get(), message_manager.get(), false);
|
|
if (ret != ErrorCode::OK) {
|
|
LOG(ERROR) << "Failed to init can receiver.";
|
|
}
|
|
|
|
// 2 == 创建协议 ===
|
|
auto canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(sender, message_manager);
|
|
|
|
// 3 === 创建电机 ===
|
|
auto motor = std::make_shared<Ti5Motor>(cfg,id);
|
|
motor->setProtocol(canopen_protocol);
|
|
|
|
// 4 === 添加电机到 MotorManager===
|
|
auto manager = std::make_shared<MotorManager>();
|
|
manager->addMotor(id,motor);
|
|
|
|
|
|
// 5 === 启动通讯 ===
|
|
can_client->start();
|
|
ret = sender->Start();
|
|
if (ret != ErrorCode::OK) {
|
|
LOG(ERROR) << "Failed to start can sender.";
|
|
}
|
|
|
|
ret = receiver->Start();
|
|
if (ret != ErrorCode::OK) {
|
|
LOG(ERROR) << "Failed to start can receiver.";
|
|
}
|
|
|
|
// 6 === 控制电机 ===
|
|
auto motor_3 = manager->getMotor(id);
|
|
motor_3->init();
|
|
|
|
// motor_3->calibrateZeroQ();
|
|
|
|
|
|
|
|
|
|
// motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY);
|
|
// motor_3->setTarget(-0.5);
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
// motor_3->setTarget(0,1.6);
|
|
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
//
|
|
// motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
// motor_3->setTarget(0,1.6);
|
|
// motor_3->calibrateZeroQ();
|
|
// motor_3->setMode(RUN_MODE_PROFILE_POSITION);
|
|
// motor_3->setLimitQ(30.14,-40.14);
|
|
// motor_3->setQ(-30);
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
// motor_3->brake();
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
// motor_3->setQ(-30);
|
|
// motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
// motor_3->setQ(-30.14);
|
|
// manager->init(id);
|
|
// manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
// manager->setQ(id,3.14);
|
|
|
|
while (true) {
|
|
|
|
auto mode = motor_3->getMode();
|
|
auto q = motor_3->getQ();
|
|
auto qd = motor_3->getQd();
|
|
std::cout << q << ", " << qd << std::endl;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
// motor_3->getQ(q);
|
|
|
|
}
|
|
|
|
LOG(INFO) << "Testing MotorManger";
|
|
} |