From 569bea33a85ad55f5a22049e8de75d9a9ca9a6f0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Mon, 19 Oct 2015 14:17:24 +0000
Subject: [PATCH] Solved a couple of bugs in the computation of the speed and
effort feedback. Both in the single and group classes.
---
src/dynamixel_motor.cpp | 8 ++++----
src/dynamixel_motor_group.cpp | 10 ++++------
2 files changed, 8 insertions(+), 10 deletions(-)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 05a9da3..da30e7f 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1233,7 +1233,9 @@ double CDynamixelMotor::get_current_speed(void)
{
try{
this->dynamixel_dev->read_word_register(this->registers[current_speed],&data);
- c_speed = this->to_speeds(data);
+ c_speed = this->to_speeds(data&0x03FF);
+ if(data&0x0400)
+ c_speed*=-1.0;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
@@ -1312,10 +1314,8 @@ double CDynamixelMotor::get_current_effort(void)
try{
this->dynamixel_dev->read_byte_register(this->registers[current_load],&data);
c_effort = (double)((data&0x3FF)*100.0/1023);
- if (this->get_current_speed() < 0)
- {
+ if(data&0x0400)
c_effort *= -1.0;
- }
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms)
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 0e86391..0e48af0 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1211,7 +1211,9 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
{
try{
this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&data);
- speeds[i] = this->to_speeds(i, data);
+ speeds[i] = this->to_speeds(i, data&0x03FF);
+ if(data&0x0400)
+ speeds[i]*=-1.0;
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms[i])
@@ -1225,7 +1227,6 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
{
unsigned short int c_effort;
- unsigned short int c_speed;
efforts.resize(this->servo_id.size());
@@ -1240,12 +1241,9 @@ void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
{
try{
this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load],&c_effort);
- this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&c_speed);
efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
- if (this->to_speeds(i, c_speed) < 0)
- {
+ if (c_effort&0x0400)
efforts[i] *= -1.0;
- }
}catch(CDynamixelAlarmException &e){
/* handle dynamixel exception */
if(e.get_alarm()&this->alarms[i])
--
GitLab