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(); // <20><> QString ת<><D7AA>Ϊ QByteArray
|
|||
|
|
qint64 bytesWritten = m_pSerialPort->write(commandData);
|
|||
|
|
|
|||
|
|
/*if (bytesWritten == -1) {
|
|||
|
|
qDebug() << "д<EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD>:" << m_pSerialPort->errorString();
|
|||
|
|
}
|
|||
|
|
else if (bytesWritten != commandData.size()) {
|
|||
|
|
qDebug() << "д<EFBFBD>벻<EFBFBD><EFBFBD><EFBFBD><EFBFBD>";
|
|||
|
|
}
|
|||
|
|
else {
|
|||
|
|
qDebug() << "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͳɹ<EFBFBD>";
|
|||
|
|
}*/
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
//<2F><>ʽ1<CABD><31>
|
|||
|
|
/*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);*/
|
|||
|
|
|
|||
|
|
//<2F><>ʽ2<CABD><32>
|
|||
|
|
//ff00ffffff000200000000000000391f6900000d42030001feff00ff000200000000000000391f6900000d42030001feff0002043458000000003c745e00000d402b001afeff00ff0002000000000000003d3b7200000d4207003efeff00020c34580000000039076e00000d4023005cfeff00ff00020000000000000038411900000d420f0022feff000204345800000000477e7700000d402b0042feff00ff00020000000000000048456a00000d420b0021feff00020434245400000049006d00000d400b0020feff00ff000200000000000000493c3100000d4203000afeff0002043458000000004d444b00000d402f004afe
|
|||
|
|
//ff00ffffffff000200000000000000112d6f00000c42070018fe
|
|||
|
|
QList<QByteArray> validFrames;
|
|||
|
|
int start = 0;
|
|||
|
|
|
|||
|
|
while (start < buffer.size())
|
|||
|
|
{
|
|||
|
|
// <20><><EFBFBD><EFBFBD>֡ͷ 'ff'
|
|||
|
|
int frameStart = buffer.indexOf(MOTOR_SYNC, start);
|
|||
|
|
if (buffer.at(frameStart + 1) == MOTOR_SYNC)
|
|||
|
|
continue;
|
|||
|
|
if (frameStart == -1)
|
|||
|
|
{
|
|||
|
|
break; // û<><C3BB><EFBFBD>ҵ<EFBFBD>֡ͷ<D6A1><CDB7><EFBFBD>˳<EFBFBD>ѭ<EFBFBD><D1AD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>֡β 'fe'
|
|||
|
|
int frameEnd = buffer.indexOf(MOTOR_ETX, frameStart + 1);
|
|||
|
|
if (frameEnd == -1)
|
|||
|
|
{
|
|||
|
|
break; // û<><C3BB><EFBFBD>ҵ<EFBFBD>֡β<D6A1><CEB2><EFBFBD>˳<EFBFBD>ѭ<EFBFBD><D1AD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><>ȡ<EFBFBD><C8A1>Ч֡<D0A7><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֡ͷ<D6A1><CDB7>֡β<D6A1><CEB2>
|
|||
|
|
QByteArray frame = buffer.mid(frameStart, frameEnd - frameStart + 1);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>21
|
|||
|
|
if (frame.size() == 21)
|
|||
|
|
{
|
|||
|
|
validFrames.append(frame);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>֡
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
//<2F><>ʽ3<CABD><33><EFBFBD>ӽ<EFBFBD>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>Ч֡<D0A7><D6A1>1<EFBFBD><31><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD>fe<66><65>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>fe<66><65>ʼ<EFBFBD><CABC>ǰ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>Ч֡<D0A7><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ21<32><31>
|
|||
|
|
|
|||
|
|
//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";//<2F>ò<EFBFBD><C3B2><EFBFBD>
|
|||
|
|
////sendCommand2Motor(cmd);
|
|||
|
|
|
|||
|
|
////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//cmd = "cfg zmd=2\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg snr=0\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg osv=0\n";//<2F><><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg zsd=3000\n";//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg zsp=2400\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
|
|||
|
|
////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//cmd = "cfg msr=1\n";//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg msv=0\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg psr=2\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
//cmd = "cfg psv=0\n";
|
|||
|
|
//sendCommand2Motor(cmd);
|
|||
|
|
|
|||
|
|
////<2F>Ӽ<EFBFBD><D3BC>ٶȺ͵<C8BA><CDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//cmd = "cfg mcs=7\n";//<2F><><EFBFBD><EFBFBD>ϸ<EFBFBD><CFB8>
|
|||
|
|
//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;
|
|||
|
|
}
|