From fbed73da01d0df8d77dabfa0d96c961895e636b4 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 3 Aug 2018 08:45:04 +0200
Subject: [PATCH] Solved a bug in the get_servo_angle() function: the registers
address for the desired servo was not generated properly. Solved a bug in the
interpretation of negatives values of the current servo angles.
---
src/darwin_robot.cpp | 6 +++---
src/examples/darwin_imu_test.cpp | 6 +++---
src/examples/darwin_joint_motion_test.cpp | 2 ++
3 files changed, 8 insertions(+), 6 deletions(-)
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 9f512d9..ca6c693 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -841,7 +841,7 @@ std::vector<double> CDarwinRobot::mm_get_servo_angles(void)
this->robot_device->read_registers(DARWIN_MM_SERVO0_CUR_POS_L,(unsigned char *)values,MAX_NUM_SERVOS*2);
for(i=0;i<MAX_NUM_SERVOS;i++)
- angles[i]=((double)values[i])/128.0;
+ angles[i]=((double)(signed short int)values[i])/128.0;
return angles;
}
@@ -860,8 +860,8 @@ double CDarwinRobot::mm_get_servo_angle(unsigned char servo_id)
{
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
- this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
- angle=((double)value)/128.0;
+ this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id*2,&value);
+ angle=((double)(signed short int)value)/128.0;
return angle;
}
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index c48e3b4..9c54c93 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -3,8 +3,8 @@
#include <iostream>
-//std::string robot_device="A603LOBS";
-std::string robot_device="A4008atn";
+std::string robot_device="/dev/pts/20";
+//std::string robot_device="A4008atn";
int main(int argc, char *argv[])
{
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
double accel_x,accel_y,accel_z;
try{
- CDarwinRobot darwin("Darwin",robot_device,1000000,0x02);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0x02,true);
std::cout << "found darwin controller" << std::endl;
std::cout << "Number of calibration samples: " << darwin.imu_get_cal_samples() << std::endl;
darwin.imu_start();
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index 6008e02..e86d584 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -45,6 +45,7 @@ int main(int argc, char *argv[])
while(darwin.joints_are_moving(JOINTS_GRP0))
{
std::cout << "moving joints ..." << std::endl;
+ std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
usleep(100000);
}
servos.clear();
@@ -60,6 +61,7 @@ int main(int argc, char *argv[])
while(darwin.joints_are_moving(JOINTS_GRP0))
{
std::cout << "moving joints ..." << std::endl;
+ std::cout << "current angle: " << darwin.mm_get_servo_angle(19) << std::endl;
usleep(100000);
}
}
--
GitLab