-
Notifications
You must be signed in to change notification settings - Fork 0
/
encoder1.cpp
84 lines (68 loc) · 1.7 KB
/
encoder1.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
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
#include "encoder.h"
#include <QString>
Encoder::Encoder(QObject *parent) :
QObject(parent)
{
rawData = "";
steps2mm = 0;
// TODO - initialization from configuration file
position = 0;
origin = 0;
}
Encoder::~Encoder()
{
}
uint16_t Encoder::steps_to_mm(uint16_t dataInStepsUnits)
{
return dataInStepsUnits * 2000 / 4096;
}
void Encoder::Update(QByteArray dataFromEncoder)
{
if (QString(dataFromEncoder).mid(0, 9) == "response_"){
rawData = dataFromEncoder;
position = ((uint8_t)rawData[9])<<4 | (((uint8_t)rawData[10])>>4);
steps2mm = (uint8_t)rawData[14];
if ((steps2mm == 0) && (position < origin)) {
origin = position;
position = 0;
} else {
position += -origin;
}
}
}
void Encoder::UpdateCoordinate(QByteArray coordData)
{
if (QString(coordData).mid(0, 9) == "response_"){
position = ((uint8_t)coordData[9])<<8 | ((uint8_t)coordData[10]);
steps2mm = (uint8_t)coordData[11];
if ((steps2mm == 0) && (position < origin)) {
origin = position;
position = 0;
} else {
position += -origin;
}
emit MotorCoordinateUpdated(position);
}
}
void Encoder::UpdateOrigin(QByteArray coordData)
{
// origin = ((uint8_t)coordData[0]) | (((uint8_t)coordData[1])<<8);
origin = ((uint8_t)coordData[9])<<8 | ((uint8_t)coordData[10]);
}
int Encoder::GetPosition()
{
return steps_to_mm(position);
}
uint8_t Encoder::GetSteps2mm()
{
return steps2mm;
}
void Encoder::ResetSteps2mm()
{
steps2mm = 0;
}
uint16_t Encoder::GetOrigin()
{
return steps_to_mm(origin);
}
void Encoder::MotorCoordinateUpdated(uint16_t) {}