first
This commit is contained in:
357
VinceMotorControllerUsb.cpp
Normal file
357
VinceMotorControllerUsb.cpp
Normal file
@ -0,0 +1,357 @@
|
||||
#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() << "д<><D0B4>ʧ<EFBFBD><CAA7>:" << m_pSerialPort->errorString();
|
||||
}
|
||||
else if (bytesWritten != commandData.size()) {
|
||||
qDebug() << "д<>벻<EFBFBD><EBB2BB><EFBFBD><EFBFBD>";
|
||||
}
|
||||
else {
|
||||
qDebug() << "<22><><EFBFBD><EFBFBD>ͳɹ<CDB3>";
|
||||
}*/
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user