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();
}
QString MainWindow::getJointAngles (const QString& address) {
auto it = m_connectedRobots.find(address);
if (m_connectedRobots.end() == it) {
qDebug() << "(barobolab) ERROR: getJointAngles on disconnected " << address << '\n';
return "[]";
}
double a1, a2, a3, a4;
if (-1 == Mobot_getJointAngles(it->second, &a1, &a2, &a3, &a4)) {
qDebug() << "(barobolab) ERROR: Mobot_getJointAngles\n";
return "[]";
}
QString angles;
angles.sprintf("[ %f, %f, %f, %f ]", a1, a2, a3, a4);
qDebug() << angles << '\n';
return angles;
}