diff --git a/src/dynamixel_robot_gazebo.cpp b/src/dynamixel_robot_gazebo.cpp
index ebf54b376ee1672ebf3240a14c7b2e65fe3c9558..ad32ccf511141268c5de4ea355193444fdc79604 100644
--- a/src/dynamixel_robot_gazebo.cpp
+++ b/src/dynamixel_robot_gazebo.cpp
@@ -166,9 +166,9 @@ namespace dynamixel_robot_gazebo
try{
dynamixel_nh.getParam("protocol_version", protocol);
if(protocol==1)
- this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version1);
+ this->dyn_slave=new CDynamixelSlaveSerial(name+"_slave",device_name,dyn_version1);
else if(protocol==2)
- this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version2);
+ this->dyn_slave=new CDynamixelSlaveSerial(name+"_slave",device_name,dyn_version2);
else
{
ROS_ERROR_STREAM("Dynamixel gazebo plugin: Invalid protocol version");
diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 3a72ed4b3d1cb3e7ffe8ff3eac7dcd68311d19fc..0a054937d11f2a44ef01968dc1027c73a64a2175 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -114,6 +114,11 @@ namespace dynamixel_robot_gazebo
}
else if(model=="XL320")
{
+ this->encoder_res=1023;
+ this->max_angle=300.0;
+ this->gear_ratio=238.0;
+ this->max_torque=0.39;
+ this->max_speed=684.0;
this->init_pid(joint_name,controller_nh);
}
else if(model=="XL430")
@@ -166,6 +171,12 @@ namespace dynamixel_robot_gazebo
*((unsigned short int *)&this->memory[82])=gains.d_gain_;
*((unsigned short int *)&this->memory[84])=gains.d_gain_;
}
+ else if(this->model=="XL320")
+ {
+ this->memory[27]=gains.d_gain_;
+ this->memory[28]=gains.i_gain_;
+ this->memory[29]=gains.p_gain_;
+ }
}
}
@@ -326,6 +337,33 @@ namespace dynamixel_robot_gazebo
this->memory[35]=3;
}
}
+ else if(model=="XL320")
+ {
+ this->memory[0]=94;
+ this->memory[1]=1;
+ this->memory[3]=this->id;
+ this->memory[4]=1;
+ this->memory[5]=250;
+ this->memory[8]=255;
+ this->memory[9]=3;
+ this->memory[11]=2;
+ this->memory[12]=65;
+ this->memory[13]=60;
+ this->memory[13]=90;
+ this->memory[15]=255;
+ this->memory[16]=3;
+ this->memory[17]=2;
+ this->memory[18]=7;
+ // initialize RAM
+ this->memory[27]=0;
+ this->memory[28]=0;
+ this->memory[29]=32;
+ this->memory[30]=255;
+ this->memory[31]=1;
+ this->memory[35]=255;
+ this->memory[36]=3;
+ this->memory[48]=32;
+ }
else
{
this->memory[7]=this->id;
@@ -419,6 +457,11 @@ namespace dynamixel_robot_gazebo
this->memory[36]=value&0x000000FF;
this->memory[37]=(value>>8)&0x000000FF;
}
+ else if(this->model=="XL320")
+ {
+ this->memory[37]=value&0x000000FF;
+ this->memory[38]=(value>>8)&0x000000FF;
+ }
else
{
this->memory[132]=value&0x000000FF;
@@ -439,7 +482,7 @@ namespace dynamixel_robot_gazebo
this->memory[38]=value&0x000000FF;
this->memory[39]=(value>>8)&0x000000FF;
}
- else if (model=="MX12W")
+ else if(model=="MX12W")
{
value/=0.916;
this->memory[38]=value&0x000000FF;
@@ -451,6 +494,12 @@ namespace dynamixel_robot_gazebo
this->memory[38]=value&0x000000FF;
this->memory[39]=(value>>8)&0x000000FF;
}
+ else if(this->model=="XL320")
+ {
+ value/=0.111;
+ this->memory[39]=value&0x000000FF;
+ this->memory[40]=(value>>8)&0x000000FF;
+ }
else
{
value/=0.229;
@@ -474,6 +523,11 @@ namespace dynamixel_robot_gazebo
this->memory[40]=value&0x000000FF;
this->memory[41]=(value>>8)&0x000000FF;
}
+ else if(this->model=="XL320")
+ {
+ this->memory[41]=value&0x000000FF;
+ this->memory[42]=(value>>8)&0x000000FF;
+ }
else
{
this->memory[124]=value&0x000000FF;
@@ -486,7 +540,7 @@ namespace dynamixel_robot_gazebo
unsigned int value=0;
double angle;
- if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
+ 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);
@@ -500,19 +554,19 @@ namespace dynamixel_robot_gazebo
unsigned int value=0;
double speed;
- if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
+ if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
{
- this->memory[32]=value&0x000000FF;
- this->memory[33]=(value>>8)&0x000000FF;
+ value=this->memory[32];
+ value+=this->memory[33]<<8;
}
else
{
- this->memory[104]=value&0x000000FF;
- this->memory[105]=(value>>8)&0x000000FF;
- this->memory[106]=(value>>16)&0x000000FF;
- this->memory[107]=(value>>24)&0x000000FF;
+ value=this->memory[104];
+ value+=this->memory[105]<<8;
+ value+=this->memory[106]<<16;
+ value+=this->memory[107]<<24;
}
- if(model=="AX12W" || model=="AX12A" || model=="AX18A")
+ if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="XL320")
value*=0.111;
else if (model=="MX12W")
value*=0.916;
@@ -527,10 +581,12 @@ namespace dynamixel_robot_gazebo
void CDynServo::on_ping(void)
{
+ std::cout << "device " << (int)this->id << " pinged" << std::endl;
}
unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
{
+ std::cout << "device " << (int)this->id << " read operation" << std::endl;
for(unsigned int i=0;i<length;i++)
data[i]=this->memory[address+i];
@@ -539,6 +595,7 @@ namespace dynamixel_robot_gazebo
unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
{
+ std::cout << "device " << (int)this->id << " write operation" << std::endl;
for(unsigned int i=0;i<length;i++)
this->memory[address+i]=data[i];
return 0;