Newer
Older
#include "machine.h"
#include <QMetaEnum>
#include <QDebug>
#include <QCoreApplication>
#include "OE_document.h"
Machine::Machine(unsigned msTimeout, unsigned retries) :
connected(false), sequence(0), sequenceAck(0), msTimeout(msTimeout), retries(retries)
{
connect(&serial, &QSerialPort::readyRead, this, &Machine::handleReadyRead);
connect(&serial, &QSerialPort::errorOccurred, this, &Machine::handleError);
connect(&timerAlive, &QTimer::timeout, this, &Machine::handleAliveTimeout);
}
Machine::~Machine()
{
disconnect(&serial, &QSerialPort::readyRead, this, &Machine::handleReadyRead);
disconnect(&serial, &QSerialPort::errorOccurred, this, &Machine::handleError);
}
void Machine::connectPort(QString portname)
{
if (!portname.isEmpty())
portName = portname;
if (serial.isOpen())
serial.close();
serial.setPortName(portName);
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
timerAlive.start(1000);
}
void Machine::disconnectPort()
{
serial.readAll();
serial.close();
timerAlive.stop();
connected = false;
emit connectionChanged(connected);
}
static uint16_t crc16(const uint8_t* data, int len)
{
static uint16_t crctable[] = {0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF, 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7, 0x0919, 0x1890, 0x2A0B, 0x3B82, 0x4F3D, 0x5EB4, 0x6C2F, 0x7DA6, 0x8551, 0x94D8, 0xA643, 0xB7CA, 0xC375, 0xD2FC, 0xE067, 0xF1EE, 0x1232, 0x03BB, 0x3120, 0x20A9, 0x5416, 0x459F, 0x7704, 0x668D, 0x9E7A, 0x8FF3, 0xBD68, 0xACE1, 0xD85E, 0xC9D7, 0xFB4C, 0xEAC5, 0x1B2B, 0x0AA2, 0x3839, 0x29B0, 0x5D0F, 0x4C86, 0x7E1D, 0x6F94, 0x9763, 0x86EA, 0xB471, 0xA5F8, 0xD147, 0xC0CE, 0xF255, 0xE3DC, 0x2464, 0x35ED, 0x0776, 0x16FF, 0x6240, 0x73C9, 0x4152, 0x50DB, 0xA82C, 0xB9A5, 0x8B3E, 0x9AB7, 0xEE08, 0xFF81, 0xCD1A, 0xDC93, 0x2D7D, 0x3CF4, 0x0E6F, 0x1FE6, 0x6B59, 0x7AD0, 0x484B, 0x59C2, 0xA135, 0xB0BC, 0x8227, 0x93AE, 0xE711, 0xF698, 0xC403, 0xD58A, 0x3656, 0x27DF, 0x1544, 0x04CD, 0x7072, 0x61FB, 0x5360, 0x42E9, 0xBA1E, 0xAB97, 0x990C, 0x8885, 0xFC3A, 0xEDB3, 0xDF28, 0xCEA1, 0x3F4F, 0x2EC6, 0x1C5D, 0x0DD4, 0x796B, 0x68E2, 0x5A79, 0x4BF0, 0xB307, 0xA28E, 0x9015, 0x819C, 0xF523, 0xE4AA, 0xD631, 0xC7B8, 0x48C8, 0x5941, 0x6BDA, 0x7A53, 0x0EEC, 0x1F65, 0x2DFE, 0x3C77, 0xC480, 0xD509, 0xE792, 0xF61B, 0x82A4, 0x932D, 0xA1B6, 0xB03F, 0x41D1, 0x5058, 0x62C3, 0x734A, 0x07F5, 0x167C, 0x24E7, 0x356E, 0xCD99, 0xDC10, 0xEE8B, 0xFF02, 0x8BBD, 0x9A34, 0xA8AF, 0xB926, 0x5AFA, 0x4B73, 0x79E8, 0x6861, 0x1CDE, 0x0D57, 0x3FCC, 0x2E45, 0xD6B2, 0xC73B, 0xF5A0, 0xE429, 0x9096, 0x811F, 0xB384, 0xA20D, 0x53E3, 0x426A, 0x70F1, 0x6178, 0x15C7, 0x044E, 0x36D5, 0x275C, 0xDFAB, 0xCE22, 0xFCB9, 0xED30, 0x998F, 0x8806, 0xBA9D, 0xAB14, 0x6CAC, 0x7D25, 0x4FBE, 0x5E37, 0x2A88, 0x3B01, 0x099A, 0x1813, 0xE0E4, 0xF16D, 0xC3F6, 0xD27F, 0xA6C0, 0xB749, 0x85D2, 0x945B, 0x65B5, 0x743C, 0x46A7, 0x572E, 0x2391, 0x3218, 0x0083, 0x110A, 0xE9FD, 0xF874, 0xCAEF, 0xDB66, 0xAFD9, 0xBE50, 0x8CCB, 0x9D42, 0x7E9E, 0x6F17, 0x5D8C, 0x4C05, 0x38BA, 0x2933, 0x1BA8, 0x0A21, 0xF2D6, 0xE35F, 0xD1C4, 0xC04D, 0xB4F2, 0xA57B, 0x97E0, 0x8669, 0x7787, 0x660E, 0x5495, 0x451C, 0x31A3, 0x202A, 0x12B1, 0x0338, 0xFBCF, 0xEA46, 0xD8DD, 0xC954, 0xBDEB, 0xAC62, 0x9EF9, 0x8F70};
uint16_t crc = 0xFFFF;
for (int i=0; i<len; i++)
crc = static_cast<uint16_t>(crc << 8) ^ crctable[((crc >> 8) ^ data[i])];
return crc;
}
static bool checkcrc(const uint8_t* data)
{
uint16_t crc = crc16(&data[1], 37-1-2);
return data[37-2]==((crc>>8)&0xff) and data[37-1]==(crc&0xff);
}
// Serial port management
void Machine::handleReadyRead()
{
msgBuffer.append(serial.readAll());
// if data is too old remove it
if (msgBuffer.size()>=3*37)
msgBuffer.remove(0, msgBuffer.size()-37);
while (msgBuffer.size() >= 37)
{
// Lock on sfd
int isfd = msgBuffer.indexOf(0x3A);
if (isfd == -1)
{
msgBuffer.clear();
continue;
}
else if (isfd != 0)
{
msgBuffer.remove(0, isfd);
}
// Check integrity
if (msgBuffer.size()>=37 && checkcrc(reinterpret_cast<uint8_t*>(msgBuffer.data())))
{
onMsg(reinterpret_cast<uint8_t*>(msgBuffer.data()));
msgBuffer.remove(0, 37);
}
else
{ // Remove old sfd
msgBuffer.remove(0, 1);
}
}
}
void Machine::handleError(QSerialPort::SerialPortError serialPortError)
{
const char* strError = QMetaEnum::fromType<QSerialPort::SerialPortError>().key(serialPortError);
if (serialPortError == QSerialPort::SerialPortError::ResourceError)
{
serial.close();
if (!timerAlive.isActive())
handleAliveTimeout();
}
else if (serialPortError != QSerialPort::SerialPortError::NoError && strError)
qDebug() << "Machine::handleError: " << strError;
}
void Machine::handleAliveTimeout()
{
if (connected)
{
connected = false;
emit connectionChanged(connected);
}
if (!serial.isOpen())
{
serial.setPortName(portName);
if (!serial.open(QIODevice::ReadWrite))
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
timerAlive.start(1000);
}
}
bool Machine::ack()
{
return sequence == sequenceAck;
}
void Machine::deletePoints() { sendCommand(Command::kDeletePoints); }
void Machine::stopMove() { sendCommand(Command::kStopMove); }
void Machine::startMove() { sendCommand(Command::kStartMove); }
void Machine::setParam() { sendCommand(Command::kSetParam); }
void Machine::gotoXY(int16_t x, int16_t y)
{
//writePosType(sndBuffer, x, 0);
//writePosType(sndBuffer, y, 1);
sendCommand(Command::kGotoXY);
}
void Machine::movedXdY(int16_t dx, int16_t dy)
{
//writePosType(sndBuffer, dx, 0);
//writePosType(sndBuffer, dy, 1);
sendCommand(Command::kMovedXdY);
}
void Machine::gotoNextPoint() { sendCommand(Command::kGotoNextPoint); }
void Machine::setNPoint(uint16_t nPoint)
{
//sndBuffer[0] = nPoint>>8;
//sndBuffer[1] = nPoint&0xff;
sendCommand(Command::kSetNPoint);
}
void Machine::setPos(int16_t x, int16_t y)
{
//writePosType(sndBuffer, x, 0);
//writePosType(sndBuffer, y, 1);
sendCommand(Command::kSetPos);
}
void Machine::sendCommand(Command cmd, uint8_t len)
{
uint8_t u8 = ++sequence;
cmdBuffer.insert(0, '\x3A'); // SFD
cmdBuffer.insert(1, *reinterpret_cast<char*>(&u8)); // Sequence
len = len ? (len<=16 ? len-1 : 15) : 0;
u8 = static_cast<uint8_t>(len<<4) | static_cast<uint8_t>(cmd);
cmdBuffer.insert(2, *reinterpret_cast<char*>(&u8)); // Length and command
if (cmdBuffer.size()<35)
cmdBuffer.append(35-cmdBuffer.size(),'\0');
uint16_t crc = crc16(reinterpret_cast<uint8_t*>(cmdBuffer.data())+1, 34);
u8 = static_cast<uint8_t>((crc>>8)&0xff);
cmdBuffer.append(*reinterpret_cast<char*>(&u8));
u8 = static_cast<uint8_t>(crc&0xff);
cmdBuffer.append(*reinterpret_cast<char*>(&u8));
serial.write(cmdBuffer);
serial.flush();
cmdBuffer.clear();
struct timespec dt;
unsigned ms, tries;
for (tries=0; (tries<retries) && (sequence != sequenceAck); tries++)
{
for (ms=0; (ms<msTimeout) && (sequence != sequenceAck); ms++)
{
dt.tv_sec = 0;
dt.tv_nsec = 1000000;
while(nanosleep(&dt,&dt)<0 && errno==EINTR);
if (sequence != sequenceAck)
qDebug() << "Machine command timeout";
}
void Machine::onMsg(uint8_t* buffer)
{
SlaveInfo oldInfo;
memcpy(&oldInfo, &info, sizeof(info));
memcpy(&info, buffer+1/*skip sfd*/, sizeof(info)); // TODO detect if big endian and fallback to something else
sequenceAck = info.sequence;
bool wasConnected = connected;
if (!wasConnected)
{
connected = true;
emit connectionChanged(connected);
}
timerAlive.start(1000);
if (memcmp(&oldInfo, &info, sizeof(info)) || !wasConnected)
{
printf("Seq %u, status %u, spaceSize %u, freeSpace %u, pos (%d,%d), nPoint %u\n",
info.sequence, info.status, info.spaceSize, info.freeSpace, info.posX, info.posY, info.nPoint);
emit stateChanged(info);
}
}
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
void Machine::loadpoints(OE_document* document)
{
int oldX=0, oldY=0;
unsigned sndPoint=0;
uint8_t u1, u2, u3;
qDebug() << "sendInstPoint" << document->instPoints.size() << "points";
if (document->instPoints.size()*3+5 > info.freeSpace)
{
qDebug() << "Too much points to fit in ram";
return;
}
//TODO stopMove();
//TODO setPos(0, 0);
//TODO setNPoint(0);
//TODO deletePoints();
// TODO update document->curPoint = comm.info.nPoint;
int x, y, dx, dy;
cmdBuffer.clear();
while (sndPoint<document->instPoints.size())
{
x = static_cast<int>(roundf( document->instPoints.at(sndPoint).x));
y = static_cast<int>(roundf(-document->instPoints.at(sndPoint).y));
dx = x-oldX;
dy = y-oldY;
u1 = static_cast<uint8_t>((abs(dx)>>8)<<2) | static_cast<uint8_t>(abs(dy)>>8);
u2 = abs(dx)&0xff;
u3 = abs(dy)&0xff;
if (dx<0)
u1 |= 0x20;
if (dy<0)
u1 |= 0x10;
cmdBuffer.append(*reinterpret_cast<char*>(&u1));
cmdBuffer.append(*reinterpret_cast<char*>(&u2));
cmdBuffer.append(*reinterpret_cast<char*>(&u3));
if ((cmdBuffer.size()+3 >= 37-5) || (sndPoint-1 == document->instPoints.size()))
{
sendCommand(Command::kAddPoints, static_cast<uint8_t>(cmdBuffer.size()/3));
}
sndPoint++;
oldX = x;
oldY = y;
}
}