358 lines
8.1 KiB
C++
358 lines
8.1 KiB
C++
#include "VinceMotorControllerUsb.h"
|
||
|
||
VinceMotorControllerUsb::VinceMotorControllerUsb(QString serialPortNumber, QString paramJson, QObject* parent)
|
||
:QObject(parent)
|
||
{
|
||
m_pSerialPort = new QSerialPort(this);
|
||
|
||
m_pSerialPort->setPortName(serialPortNumber);
|
||
m_pSerialPort->setReadBufferSize(512);
|
||
|
||
bool bRes = m_pSerialPort->setBaudRate(9600);
|
||
if (!bRes)
|
||
{
|
||
//qDebug() << "Err:setBaudRate Failed.Exit Code:1";
|
||
//std::cout << "Err.setBaudRate Failed" << std::endl;
|
||
printf("Err:setBaudRate Failed.Exit Code:1");
|
||
}
|
||
|
||
bRes = m_pSerialPort->open(QIODevice::ReadWrite);
|
||
if (!bRes)
|
||
{
|
||
//qDebug() << "Err:open Failed.Exit Code:2";
|
||
//std::cout << "Err.open Failed" << std::endl;
|
||
printf("Err:open Failed.Exit Code:2");
|
||
}
|
||
|
||
m_iSpeed = 3000;
|
||
|
||
init(paramJson);
|
||
}
|
||
|
||
VinceMotorControllerUsb::~VinceMotorControllerUsb()
|
||
{
|
||
delete m_pSerialPort;
|
||
}
|
||
|
||
int VinceMotorControllerUsb::sendCommand2Motor(QString cmd)
|
||
{
|
||
QByteArray commandData = cmd.toUtf8(); // 将 QString 转换为 QByteArray
|
||
qint64 bytesWritten = m_pSerialPort->write(commandData);
|
||
|
||
/*if (bytesWritten == -1) {
|
||
qDebug() << "写入失败:" << m_pSerialPort->errorString();
|
||
}
|
||
else if (bytesWritten != commandData.size()) {
|
||
qDebug() << "写入不完整";
|
||
}
|
||
else {
|
||
qDebug() << "命令发送成功";
|
||
}*/
|
||
|
||
return bytesWritten;
|
||
}
|
||
|
||
int VinceMotorControllerUsb::recvData(QByteArray& dataRecv)
|
||
{
|
||
dataRecv.clear();
|
||
|
||
QByteArray temp;
|
||
|
||
temp = m_pSerialPort->readAll();
|
||
dataRecv.append(temp);
|
||
|
||
while (dataRecv.size() < 21)
|
||
{
|
||
m_pSerialPort->waitForReadyRead(100);
|
||
temp = m_pSerialPort->readAll();
|
||
dataRecv.append(temp);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
IrisMotorErrorCode VinceMotorControllerUsb::extractOneValidFrame(QByteArray& buffer)
|
||
{
|
||
//std::cout << "before------" << buffer.size() << std::endl;
|
||
//std::cout << buffer.toHex().toStdString() << std::endl;
|
||
|
||
IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;
|
||
|
||
//方式1：
|
||
/*int startPos = buffer.lastIndexOf('\xFF');
|
||
int endPos = buffer.indexOf('\xFE');
|
||
|
||
if (endPos <= startPos)
|
||
{
|
||
errorCode = MOTOR_INVALID_FRAME;
|
||
return errorCode;
|
||
}
|
||
|
||
buffer = buffer.mid(startPos, endPos - startPos + 1);*/
|
||
|
||
//方式2：
|
||
//ff00ffffff000200000000000000391f6900000d42030001feff00ff000200000000000000391f6900000d42030001feff0002043458000000003c745e00000d402b001afeff00ff0002000000000000003d3b7200000d4207003efeff00020c34580000000039076e00000d4023005cfeff00ff00020000000000000038411900000d420f0022feff000204345800000000477e7700000d402b0042feff00ff00020000000000000048456a00000d420b0021feff00020434245400000049006d00000d400b0020feff00ff000200000000000000493c3100000d4203000afeff0002043458000000004d444b00000d402f004afe
|
||
//ff00ffffffff000200000000000000112d6f00000c42070018fe
|
||
QList<QByteArray> validFrames;
|
||
int start = 0;
|
||
|
||
while (start < buffer.size())
|
||
{
|
||
// 查找帧头 'ff'
|
||
int frameStart = buffer.indexOf(MOTOR_SYNC, start);
|
||
if (buffer.at(frameStart + 1) == MOTOR_SYNC)
|
||
continue;
|
||
if (frameStart == -1)
|
||
{
|
||
break; // 没有找到帧头，退出循环
|
||
}
|
||
|
||
// 查找帧尾 'fe'
|
||
int frameEnd = buffer.indexOf(MOTOR_ETX, frameStart + 1);
|
||
if (frameEnd == -1)
|
||
{
|
||
break; // 没有找到帧尾，退出循环
|
||
}
|
||
|
||
// 提取有效帧（包括帧头和帧尾）
|
||
QByteArray frame = buffer.mid(frameStart, frameEnd - frameStart + 1);
|
||
|
||
// 检查帧长度是否等于21
|
||
if (frame.size() == 21)
|
||
{
|
||
validFrames.append(frame);
|
||
}
|
||
|
||
// 更新起始位置，继续查找下一个帧
|
||
start = frameEnd + 1;
|
||
}
|
||
if (validFrames.size() == 0)
|
||
{
|
||
std::cout << "error--------------: " << buffer.toHex().toStdString() << std::endl;
|
||
|
||
|
||
errorCode = MOTOR_INVALID_FRAME;
|
||
return errorCode;
|
||
}
|
||
|
||
buffer = validFrames.at(validFrames.size() - 1);
|
||
|
||
//方式3：从结尾遍历，找到有效帧（1）找到所有fe（2）从最后一个fe开始向前找到有效帧（长度为21）
|
||
|
||
//std::cout << "after" << buffer.size() << std::endl;
|
||
//std::cout << buffer.toHex().toStdString() << std::endl;
|
||
|
||
return errorCode;
|
||
}
|
||
|
||
IrisMotorErrorCode VinceMotorControllerUsb::parseOneValidFrame(QByteArray& buf, State& backdatt)
|
||
{
|
||
IrisMotorErrorCode errorCode = MOTOR_NO_ERROR;
|
||
|
||
backdatt.Speed = MOTOR_disconnection;
|
||
|
||
buf.remove(0, 3);
|
||
buf.remove(buf.length() - 3, 3);
|
||
int location;
|
||
unsigned char a[15];
|
||
memcpy(a, buf.data(), 15);
|
||
location = ((a[0] & 0x0f) << 28) | ((a[1] & 0x7f) << 21) | ((a[2] & 0x7f) << 14) | ((a[3] & 0x7f) << 7) | ((a[4] & 0x7f));
|
||
float speed;
|
||
|
||
memcpy(&speed, &location, 4);
|
||
backdatt.Speed = speed;
|
||
int loc;
|
||
loc = ((a[5] & 0x0f) << 28) | ((a[6] & 0x7f) << 21) | ((a[7] & 0x7f) << 14) | ((a[8] & 0x7f) << 7) | ((a[9] & 0x7f));
|
||
backdatt.Location = loc;
|
||
backdatt.Speed = speed;
|
||
location = ((a[10] & 0x0f) << 28) | ((a[11] & 0x7f) << 21) | ((a[12] & 0x7f) << 14) | ((a[13] & 0x7f) << 7) | ((a[14] & 0x7f));
|
||
backdatt.Stata = unsigned int(location);
|
||
|
||
/*QString str1 = "Speed " + QString::number(backdatt.Speed, 'f', 2) + " location " + QString::number(backdatt.Location);
|
||
qDebug() << str1;*/
|
||
|
||
return errorCode;
|
||
}
|
||
|
||
IrisMotorErrorCode VinceMotorControllerUsb::parseBytedata(QByteArray buf, State& backdatt)
|
||
{
|
||
IrisMotorErrorCode errorCode = extractOneValidFrame(buf);
|
||
|
||
backdatt.Speed = MOTOR_disconnection;
|
||
backdatt.Location = 0;
|
||
if (errorCode == MOTOR_NO_ERROR)
|
||
{
|
||
parseOneValidFrame(buf, backdatt);
|
||
return errorCode;
|
||
}
|
||
|
||
errorCode = MOTOR_INVALID_FRAME;
|
||
return errorCode;
|
||
}
|
||
|
||
void VinceMotorControllerUsb::init(QString jsonPath)
|
||
{
|
||
enable();
|
||
|
||
JsonOperate xxx(jsonPath);
|
||
//xxx.createJsonFile();
|
||
QStringList re = xxx.parseJsonFile();
|
||
|
||
for (const QString& pair : re)
|
||
{
|
||
//qDebug().noquote() << pair;
|
||
sendCommand2Motor(pair);
|
||
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
//QString cmd;
|
||
|
||
////cmd = "cfg s1f=2\n";//用不上
|
||
////sendCommand2Motor(cmd);
|
||
|
||
////归零设置
|
||
//cmd = "cfg zmd=2\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg snr=0\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg osv=0\n";//归零用传感器常开常闭设置
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg zsd=3000\n";//正数向原点走
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg zsp=2400\n";
|
||
//sendCommand2Motor(cmd);
|
||
|
||
////设置正负极限
|
||
//cmd = "cfg msr=1\n";//负极限
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg msv=0\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg psr=2\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg psv=0\n";
|
||
//sendCommand2Motor(cmd);
|
||
|
||
////加减速度和电流设置
|
||
//cmd = "cfg mcs=7\n";//设置细分
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg acc=20000\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg dec=20000\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg crn=4\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg cra=4\n";
|
||
//sendCommand2Motor(cmd);
|
||
//cmd = "cfg crh=1\n";
|
||
//sendCommand2Motor(cmd);
|
||
}
|
||
|
||
bool VinceMotorControllerUsb::isConnected()
|
||
{
|
||
State re = getState();
|
||
if (re.Speed == MOTOR_disconnection)
|
||
{
|
||
return false;
|
||
}
|
||
else
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
|
||
void VinceMotorControllerUsb::enable()
|
||
{
|
||
QString cmd = "ena\n";
|
||
sendCommand2Motor(cmd);
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
void VinceMotorControllerUsb::move2Pos(double pos)
|
||
{
|
||
QString cmd = "pos " + QString::number(pos, 'f', 10) + "\n";
|
||
sendCommand2Motor(cmd);
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
void VinceMotorControllerUsb::move(bool towardOrigin)
|
||
{
|
||
if (towardOrigin)
|
||
{
|
||
setSpeed(abs(m_iSpeed) * -1);
|
||
}
|
||
else
|
||
{
|
||
setSpeed(abs(m_iSpeed));
|
||
}
|
||
|
||
sendCommand2Motor("mov\n");
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
void VinceMotorControllerUsb::stopMove()
|
||
{
|
||
QString cmd = "stp\n";
|
||
sendCommand2Motor(cmd);
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
void VinceMotorControllerUsb::setSpeed(double speed)
|
||
{
|
||
m_iSpeed = speed;
|
||
QString cmd = "cfg spd=" + QString::number(speed) + "\n";
|
||
sendCommand2Motor(cmd);
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
double VinceMotorControllerUsb::getSpeed()
|
||
{
|
||
State re = getState();
|
||
return re.Speed;
|
||
}
|
||
|
||
double VinceMotorControllerUsb::getCurrentLoc()
|
||
{
|
||
State re = getState();
|
||
return re.Location;
|
||
}
|
||
|
||
void VinceMotorControllerUsb::zeroStart()
|
||
{
|
||
QString cmd = "zero start\n";
|
||
sendCommand2Motor(cmd);
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
}
|
||
|
||
int VinceMotorControllerUsb::getSpeedLocation(double& speed, double& locatioin)
|
||
{
|
||
State re = getState();
|
||
speed = re.Speed;
|
||
locatioin = re.Location;
|
||
|
||
return 0;
|
||
}
|
||
|
||
State VinceMotorControllerUsb::getState()
|
||
{
|
||
QString cmd = "sts\n";
|
||
sendCommand2Motor(cmd);
|
||
|
||
QByteArray buf;
|
||
recvData(buf);
|
||
//std::cout << buf.size() << std::endl;
|
||
//std::cout << buf.toHex().toStdString() << std::endl;
|
||
|
||
State back;
|
||
IrisMotorErrorCode errorCode = parseBytedata(buf, back);
|
||
|
||
return back;
|
||
}
|