From ae5359403298b7ab0f3ff6a242623a7983970255 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 31 Aug 2020 16:04:52 +0200
Subject: [PATCH] Added a feature to set the initial angle of the servos.
---
include/dynamixel_servo.h | 1 +
src/dynamixel_servo.cpp | 27 ++++++++++++++++++++++++++-
2 files changed, 27 insertions(+), 1 deletion(-)
diff --git a/include/dynamixel_servo.h b/include/dynamixel_servo.h
index 880d2c5..9273673 100644
--- a/include/dynamixel_servo.h
+++ b/include/dynamixel_servo.h
@@ -32,6 +32,7 @@ namespace dynamixel_robot_gazebo
void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
void init_memory(void);
void set_current_angle(double angle);
+ void set_target_angle(double angle);
void set_current_speed(double speed);
void set_current_pwm(double pwm);
double saturate_command(double command);
diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 5ca6b08..2e1fafe 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -5,6 +5,7 @@ namespace dynamixel_robot_gazebo
CDynServo::CDynServo(const std::string &joint_name,CDynamixelSlaveSerial *dyn_slave,hardware_interface::EffortJointInterface* hw,ros::NodeHandle &controller_nh)
{
int id;
+ double initial_angle;
// Joint handle
try {
@@ -140,6 +141,10 @@ namespace dynamixel_robot_gazebo
else
this->id=id;
this->init_memory();
+ // read initial angle
+ initial_angle=0.0;
+ servo_nh.getParam("initial_angle",initial_angle);
+ this->set_target_angle(initial_angle);
dyn_slave->add_slave(id,boost::bind(&CDynServo::on_ping,this),boost::bind(&CDynServo::on_read,this,_1,_2,_3),boost::bind(&CDynServo::on_write,this,_1,_2,_3));
}
@@ -471,6 +476,26 @@ namespace dynamixel_robot_gazebo
}
}
+ void CDynServo::set_target_angle(double angle)
+ {
+ unsigned int value;
+
+ value=((((angle*180.0)/3.14159)+(this->max_angle/2.0))*this->encoder_res)/this->max_angle;
+ if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
+ {
+ this->memory[30]=value&0x000000FF;
+ this->memory[31]=(value>>8)&0x000000FF;
+ }
+ else
+ {
+ this->memory[116]=value&0x000000FF;
+ this->memory[117]=(value>>8)&0x000000FF;
+ this->memory[118]=(value>>16)&0x000000FF;
+ this->memory[119]=(value>>24)&0x000000FF;
+ }
+ }
+
+
void CDynServo::set_current_speed(double speed)
{
unsigned int value;
@@ -553,7 +578,7 @@ namespace dynamixel_robot_gazebo
if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
value=this->memory[30]+(this->memory[31]<<8);
else
- value=this->memory[116]+(this->memory[133]<<8)+(this->memory[134]<<16)+(this->memory[135]<<24);
+ value=this->memory[116]+(this->memory[117]<<8)+(this->memory[118]<<16)+(this->memory[119]<<24);
angle=(((((value*this->max_angle)/this->encoder_res))-this->max_angle/2.0)*3.14159)/180.0;
return angle;
--
GitLab