-
Notifications
You must be signed in to change notification settings - Fork 0
/
encoder.cpp
91 lines (71 loc) · 1.51 KB
/
encoder.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
85
86
87
88
89
#include "encoder.h"
Encoder::Encoder(int motorID, QObject *parent) :
QObject(parent)
{
id = motorID;
rawData = "";
steps2mm = 0;
// TODO - initialization from configuration file
position = 0;
origin = 0;
}
Encoder::~Encoder()
{
}
int Encoder::GetID()
{
return id;
}
uint16_t Encoder::steps_to_mm(uint16_t dataInStepsUnits)
{
return dataInStepsUnits * 2000 / 4096;
}
void Encoder::Update(QByteArray dataFromMaster)
{
if (QString(dataFromMaster).mid(0, 9) == "response_"){
rawData = dataFromMaster;
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(uint16_t coord)
{
position = coord;
steps2mm = position/4096;
if (position < origin) {
origin = position;
position = 0;
} else {
position -= origin;
}
emit MotorCoordinateUpdated(GetID(), GetPosition());
}
void Encoder::UpdateOrigin(uint16_t coord)
{
origin = coord;
position = 0;
steps2mm = 0;
emit MotorCoordinateUpdated(GetID(), position);
}
uint16_t 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);
}