柯蒂斯控制协议，相关集成
This commit is contained in:
CaiXiang
349
src/can/CurtisMotorController.cpp
Normal file
349
src/can/CurtisMotorController.cpp
Normal file
@@ -0,0 +1,349 @@
|
||||
/**
|
||||
* @file CurtisMotorController.cpp
|
||||
* @brief 柯蒂斯电机控制器实现文件
|
||||
*/
|
||||
|
||||
#include "../include/can/CurtisMotorController.h"
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <cstring>
|
||||
#include <windows.h>
|
||||
|
||||
// ==================== 构造函数和析构函数 ====================
|
||||
|
||||
CurtisMotorController::CurtisMotorController(DWORD deviceType,
|
||||
DWORD deviceIndex,
|
||||
DWORD canChannel)
|
||||
: m_deviceType(deviceType)
|
||||
, m_deviceIndex(deviceIndex)
|
||||
, m_canChannel(canChannel)
|
||||
, m_isConnected(false)
|
||||
, m_statusCallback(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
CurtisMotorController::~CurtisMotorController() {
|
||||
Disconnect();
|
||||
}
|
||||
|
||||
// ==================== 连接管理 ====================
|
||||
|
||||
bool CurtisMotorController::Connect() {
|
||||
if (m_isConnected) {
|
||||
std::cout << "[Curtis] 警告: 已经连接" << std::endl;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::cout << "[Curtis] 正在连接 Curtis 控制器..." << std::endl;
|
||||
|
||||
// 步骤1: 打开 CAN 设备
|
||||
std::cout << "[Curtis] 1/4 打开 CAN 设备 (类型=" << m_deviceType
|
||||
<< ", 索引=" << m_deviceIndex << ")..." << std::endl;
|
||||
|
||||
DWORD ret = VCI_OpenDevice(m_deviceType, m_deviceIndex, 0);
|
||||
if (ret != 1) {
|
||||
std::cerr << "[Curtis] 错误: 打开设备失败 (返回值=" << ret << ")" << std::endl;
|
||||
return false;
|
||||
}
|
||||
std::cout << "[Curtis] ✓ 设备打开成功" << std::endl;
|
||||
|
||||
// 步骤2: 配置 CAN 参数（125kbps）
|
||||
std::cout << "[Curtis] 2/4 初始化 CAN (125kbps)..." << std::endl;
|
||||
|
||||
VCI_INIT_CONFIG config;
|
||||
config.AccCode = 0x80000008;
|
||||
config.AccMask = 0xFFFFFFFF; // 接收所有帧
|
||||
config.Filter = 1; // 接收标准帧和扩展帧
|
||||
config.Timing0 = 0x03; // 125 kbps
|
||||
config.Timing1 = 0x1C;
|
||||
config.Mode = 0; // 正常模式
|
||||
config.Reserved = 0;
|
||||
|
||||
ret = VCI_InitCAN(m_deviceType, m_deviceIndex, m_canChannel, &config);
|
||||
if (ret != 1) {
|
||||
std::cerr << "[Curtis] 错误: 初始化 CAN 失败 (返回值=" << ret << ")" << std::endl;
|
||||
VCI_CloseDevice(m_deviceType, m_deviceIndex);
|
||||
return false;
|
||||
}
|
||||
std::cout << "[Curtis] ✓ CAN 初始化成功" << std::endl;
|
||||
|
||||
// 步骤3: 启动 CAN 通道
|
||||
std::cout << "[Curtis] 3/4 启动 CAN 通道..." << std::endl;
|
||||
ret = VCI_StartCAN(m_deviceType, m_deviceIndex, m_canChannel);
|
||||
if (ret != 1) {
|
||||
std::cerr << "[Curtis] 错误: 启动 CAN 失败 (返回值=" << ret << ")" << std::endl;
|
||||
VCI_CloseDevice(m_deviceType, m_deviceIndex);
|
||||
return false;
|
||||
}
|
||||
std::cout << "[Curtis] ✓ CAN 通道启动成功" << std::endl;
|
||||
|
||||
// 步骤4: 清空缓冲区
|
||||
std::cout << "[Curtis] 4/4 清空缓冲区..." << std::endl;
|
||||
VCI_ClearBuffer(m_deviceType, m_deviceIndex, m_canChannel);
|
||||
std::cout << "[Curtis] ✓ 缓冲区清空完成" << std::endl;
|
||||
|
||||
m_isConnected = true;
|
||||
std::cout << "[Curtis] ========== 连接成功！=========" << std::endl;
|
||||
std::cout << "[Curtis] 准备通讯（请以 ≤20ms 周期发送命令）" << std::endl;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CurtisMotorController::Disconnect() {
|
||||
if (!m_isConnected) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "[Curtis] 正在断开连接..." << std::endl;
|
||||
|
||||
// 发送停止命令
|
||||
std::cout << "[Curtis] 发送停止命令..." << std::endl;
|
||||
SendStopCommand();
|
||||
Sleep(50); // 等待命令发送
|
||||
|
||||
// 关闭设备
|
||||
std::cout << "[Curtis] 关闭 CAN 设备..." << std::endl;
|
||||
VCI_CloseDevice(m_deviceType, m_deviceIndex);
|
||||
|
||||
m_isConnected = false;
|
||||
std::cout << "[Curtis] ✓ 断开连接完成" << std::endl;
|
||||
}
|
||||
|
||||
// ==================== 命令发送 ====================
|
||||
|
||||
bool CurtisMotorController::SendCommand(const CurtisCommand& cmd) {
|
||||
if (!m_isConnected) {
|
||||
std::cerr << "[Curtis] 错误: 未连接到控制器" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 限制数值范围
|
||||
int16_t driveSpeed = cmd.driveSpeed;
|
||||
if (driveSpeed < CURTIS_DRIVE_SPEED_MIN) driveSpeed = CURTIS_DRIVE_SPEED_MIN;
|
||||
if (driveSpeed > CURTIS_DRIVE_SPEED_MAX) driveSpeed = CURTIS_DRIVE_SPEED_MAX;
|
||||
|
||||
int16_t liftLeft = cmd.liftSpeedLeft;
|
||||
if (liftLeft < CURTIS_LIFT_SPEED_MIN) liftLeft = CURTIS_LIFT_SPEED_MIN;
|
||||
if (liftLeft > CURTIS_LIFT_SPEED_MAX) liftLeft = CURTIS_LIFT_SPEED_MAX;
|
||||
|
||||
int16_t liftRight = cmd.liftSpeedRight;
|
||||
if (liftRight < CURTIS_LIFT_SPEED_MIN) liftRight = CURTIS_LIFT_SPEED_MIN;
|
||||
if (liftRight > CURTIS_LIFT_SPEED_MAX) liftRight = CURTIS_LIFT_SPEED_MAX;
|
||||
|
||||
int16_t steerAngle = cmd.steerAngle;
|
||||
if (steerAngle < CURTIS_STEER_ANGLE_MIN) steerAngle = CURTIS_STEER_ANGLE_MIN;
|
||||
if (steerAngle > CURTIS_STEER_ANGLE_MAX) steerAngle = CURTIS_STEER_ANGLE_MAX;
|
||||
|
||||
// 发送帧1: 0x1E5 - 行走和提升命令
|
||||
BYTE data1[8];
|
||||
data1[0] = cmd.controlBits0;
|
||||
data1[1] = cmd.controlBits1 | CURTIS_CTRL_AGV_CONNECTED; // 确保AGV标识位
|
||||
data1[2] = driveSpeed & 0xFF;
|
||||
data1[3] = (driveSpeed >> 8) & 0xFF;
|
||||
data1[4] = liftLeft & 0xFF;
|
||||
data1[5] = (liftLeft >> 8) & 0xFF;
|
||||
data1[6] = liftRight & 0xFF;
|
||||
data1[7] = (liftRight >> 8) & 0xFF;
|
||||
|
||||
if (!SendFrame(CURTIS_ID_DRIVE_CMD, data1, 8)) {
|
||||
std::cerr << "[Curtis] 错误: 发送 0x1E5 失败" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 发送帧2: 0x2E5 - 转向命令
|
||||
BYTE data2[8] = {0};
|
||||
data2[0] = steerAngle & 0xFF;
|
||||
data2[1] = (steerAngle >> 8) & 0xFF;
|
||||
// Byte2-7 已初始化为0
|
||||
|
||||
if (!SendFrame(CURTIS_ID_STEER_CMD, data2, 8)) {
|
||||
std::cerr << "[Curtis] 错误: 发送 0x2E5 失败" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 保存最后发送的命令
|
||||
m_lastCommand = cmd;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CurtisMotorController::SendStopCommand() {
|
||||
CurtisCommand stopCmd;
|
||||
stopCmd.controlBits0 = 0x00; // 调速开关有效
|
||||
stopCmd.controlBits1 = CURTIS_CTRL_AGV_CONNECTED;
|
||||
stopCmd.driveSpeed = 0;
|
||||
stopCmd.liftSpeedLeft = 0;
|
||||
stopCmd.liftSpeedRight = 0;
|
||||
stopCmd.steerAngle = 0;
|
||||
|
||||
return SendCommand(stopCmd);
|
||||
}
|
||||
|
||||
void CurtisMotorController::EmergencyStop() {
|
||||
if (!m_isConnected) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::cout << "[Curtis] !!! 执行紧急停车 !!!" << std::endl;
|
||||
|
||||
CurtisCommand stopCmd;
|
||||
stopCmd.controlBits0 = 0x00;
|
||||
stopCmd.controlBits1 = CURTIS_CTRL_EMERGENCY_STOP | CURTIS_CTRL_AGV_CONNECTED;
|
||||
stopCmd.driveSpeed = 0;
|
||||
stopCmd.liftSpeedLeft = 0;
|
||||
stopCmd.liftSpeedRight = 0;
|
||||
stopCmd.steerAngle = 0;
|
||||
|
||||
// 连续发送3次确保收到
|
||||
for (int i = 0; i < 3; i++) {
|
||||
SendCommand(stopCmd);
|
||||
Sleep(10);
|
||||
}
|
||||
}
|
||||
|
||||
// ==================== 状态接收 ====================
|
||||
|
||||
bool CurtisMotorController::ReceiveStatus() {
|
||||
if (!m_isConnected) {
|
||||
return false;
|
||||
}
|
||||
|
||||
VCI_CAN_OBJ rxFrames[2500];
|
||||
DWORD count = VCI_Receive(m_deviceType, m_deviceIndex,
|
||||
m_canChannel, rxFrames, 2500, 0);
|
||||
|
||||
if (count == (DWORD)-1) {
|
||||
std::cerr << "[Curtis] 错误: USB 断开或设备不存在" << std::endl;
|
||||
m_isConnected = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool statusUpdated = false;
|
||||
|
||||
for (DWORD i = 0; i < count; i++) {
|
||||
if (rxFrames[i].ID == CURTIS_ID_STATUS) {
|
||||
ParseStatusFrame(rxFrames[i]);
|
||||
statusUpdated = true;
|
||||
|
||||
// 调用回调函数
|
||||
if (m_statusCallback) {
|
||||
m_statusCallback(m_status);
|
||||
}
|
||||
}
|
||||
else if (rxFrames[i].ID == CURTIS_ID_RESERVED) {
|
||||
// 0x360 保留帧，暂不处理
|
||||
}
|
||||
}
|
||||
|
||||
return statusUpdated;
|
||||
}
|
||||
|
||||
// ==================== 故障和超时检测 ====================
|
||||
|
||||
bool CurtisMotorController::IsTimeout(uint32_t timeoutMs) const {
|
||||
if (m_status.lastUpdateTime == 0) {
|
||||
return true; // 从未接收过数据
|
||||
}
|
||||
|
||||
uint32_t currentTime = GetTickCount();
|
||||
uint32_t elapsed = currentTime - m_status.lastUpdateTime;
|
||||
|
||||
return elapsed > timeoutMs;
|
||||
}
|
||||
|
||||
std::string CurtisMotorController::GetErrorString() const {
|
||||
std::ostringstream oss;
|
||||
|
||||
if (m_status.driveError != 0) {
|
||||
oss << "行走故障=0x" << std::hex << (int)m_status.driveError << " ";
|
||||
}
|
||||
if (m_status.steerError != 0) {
|
||||
oss << "转向故障=0x" << std::hex << (int)m_status.steerError << " ";
|
||||
}
|
||||
if (m_status.isEmergencyStop) {
|
||||
oss << "[急停] ";
|
||||
}
|
||||
|
||||
std::string result = oss.str();
|
||||
return result.empty() ? "无故障" : result;
|
||||
}
|
||||
|
||||
// ==================== 工具函数 ====================
|
||||
|
||||
int16_t CurtisMotorController::SpeedPercentToCAN(float percent) {
|
||||
if (percent > 100.0f) percent = 100.0f;
|
||||
if (percent < -100.0f) percent = -100.0f;
|
||||
return static_cast<int16_t>(percent * 40.95f);
|
||||
}
|
||||
|
||||
float CurtisMotorController::SpeedCANToPercent(int16_t canValue) {
|
||||
return canValue / 40.95f;
|
||||
}
|
||||
|
||||
int16_t CurtisMotorController::AngleDegreeToCAN(float degrees) {
|
||||
if (degrees > 90.0f) degrees = 90.0f;
|
||||
if (degrees < -90.0f) degrees = -90.0f;
|
||||
return static_cast<int16_t>(degrees * 10.0f);
|
||||
}
|
||||
|
||||
float CurtisMotorController::AngleCANToDegree(int16_t canValue) {
|
||||
return canValue / 10.0f;
|
||||
}
|
||||
|
||||
void CurtisMotorController::PrintStatus() const {
|
||||
std::cout << "\n========== Curtis 状态 ==========" << std::endl;
|
||||
std::cout << "行走故障: 0x" << std::hex << (int)m_status.driveError
|
||||
<< " 转向故障: 0x" << (int)m_status.steerError << std::dec << std::endl;
|
||||
std::cout << "转向角度: " << m_status.currentAngle << "°" << std::endl;
|
||||
std::cout << "电机转速: " << m_status.motorSpeed << std::endl;
|
||||
std::cout << "电量: " << (int)m_status.batteryLevel << "%" << std::endl;
|
||||
std::cout << "模式: " << (m_status.isAutoMode ? "自动" : "手动")
|
||||
<< " 急停: " << (m_status.isEmergencyStop ? "是" : "否") << std::endl;
|
||||
std::cout << "================================\n" << std::endl;
|
||||
}
|
||||
|
||||
// ==================== 内部辅助函数 ====================
|
||||
|
||||
bool CurtisMotorController::SendFrame(UINT canId, const BYTE* data, BYTE len) {
|
||||
VCI_CAN_OBJ frame;
|
||||
memset(&frame, 0, sizeof(VCI_CAN_OBJ));
|
||||
|
||||
frame.ID = canId;
|
||||
frame.SendType = 0; // 正常发送（自动重试）
|
||||
frame.RemoteFlag = 0; // 数据帧
|
||||
frame.ExternFlag = 0; // 标准帧
|
||||
frame.DataLen = len;
|
||||
|
||||
if (len > 8) len = 8;
|
||||
memcpy(frame.Data, data, len);
|
||||
|
||||
DWORD sent = VCI_Transmit(m_deviceType, m_deviceIndex,
|
||||
m_canChannel, &frame, 1);
|
||||
|
||||
return sent == 1;
|
||||
}
|
||||
|
||||
void CurtisMotorController::ParseStatusFrame(const VCI_CAN_OBJ& frame) {
|
||||
// Byte0: 行走故障代码
|
||||
m_status.driveError = frame.Data[0];
|
||||
|
||||
// Byte1: 转向故障代码
|
||||
m_status.steerError = frame.Data[1];
|
||||
|
||||
// Byte2-3: 转向角度反馈
|
||||
int16_t angleRaw = static_cast<int16_t>((frame.Data[3] << 8) | frame.Data[2]);
|
||||
m_status.currentAngle = angleRaw / 10.0f;
|
||||
|
||||
// Byte4-5: 电机转速
|
||||
m_status.motorSpeed = static_cast<int16_t>((frame.Data[5] << 8) | frame.Data[4]);
|
||||
|
||||
// Byte6: 电量
|
||||
m_status.batteryLevel = frame.Data[6];
|
||||
|
||||
// Byte7: 状态位
|
||||
m_status.isAutoMode = (frame.Data[7] & CURTIS_STATUS_AUTO_MODE) != 0;
|
||||
m_status.isEmergencyStop = (frame.Data[7] & CURTIS_STATUS_EMERGENCY_STOP) != 0;
|
||||
|
||||
// 更新时间戳
|
||||
m_status.lastUpdateTime = GetTickCount();
|
||||
}
|
||||
Reference in New Issue
Block a user