From c497ede45f6a2e03495c29c1f090317579a3398d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Thu, 8 Mar 2012 10:54:48 +0000
Subject: [PATCH] Implemented the function get_torque().
---
src/dynamixel_motor.cpp | 18 +++++++++++++++++-
1 file changed, 17 insertions(+), 1 deletion(-)
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index f36d867..40b679e 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1364,7 +1364,23 @@ void CDynamixelMotor::set_torque(float torque_ratio)
float CDynamixelMotor::get_torque(void)
{
- return 0.0;
+ unsigned short int load;
+ float torque;
+
+ if(this->dynamixel_dev==NULL)
+ {
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+ }
+ else
+ {
+ this->dynamixel_dev->read_word_register(current_load,&load);
+ torque=(load&0x3FF)*100.0/1023;
+ if(load>0x3FF)
+ torque=-1*torque;
+ }
+
+ return torque;
}
CDynamixelMotor::~CDynamixelMotor()
--
GitLab