-
Notifications
You must be signed in to change notification settings - Fork 2
/
robotlistener.cpp
66 lines (56 loc) · 1.18 KB
/
robotlistener.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
#include "robotlistener.h"
#include <QDebug>
#include <QThread>
RobotListener::RobotListener(mobot_t* robot, QString addr)
{
m_robot = robot;
m_addr = addr;
m_timer = new QTimer(this);
initialized = false;
}
RobotListener::~RobotListener()
{
stopWork();
}
void RobotListener::startWork()
{
QObject::connect(m_timer, SIGNAL(timeout()), this, SLOT(doWork()));
m_timer->start(150);
}
void RobotListener::stopWork()
{
m_timer->stop();
}
void RobotListener::doWork()
{
double curJoint2Angle;
double curJoint4Angle;
double a[4];
double delta = 20.0*M_PI/180.0;
int rc;
if(!initialized) {
rc = Mobot_getJointAngles(m_robot,
&curJoint1Angle,
&curJoint2Angle,
&curJoint3Angle,
&curJoint4Angle);
if(rc) return;
initialized = true;
}
rc = Mobot_getJointAngles(m_robot,
&a[0],
&a[1],
&a[2],
&a[3]);
if(rc) return;
//qDebug() << m_addr << curJoint1Angle << a[0] << m_robot;
if((a[0] - curJoint1Angle) > delta) {
curJoint1Angle = a[0];
emit scrollUp(m_addr);
}
if((curJoint1Angle - a[0] > delta)) {
curJoint1Angle = a[0];
emit scrollDown(m_addr);
}
QThread::yieldCurrentThread();
}