Skip to content
Snippets Groups Projects
Commit 4acf7d84 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented functions to set the current position, speed and effort. 

Implemented functions to get the target position and angle.
Completed the update() function. Servos are moving.
parent 2ac3c66d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/dynamixel_servo.h
+ 11
− 1
View file @ 4acf7d84
...@@ -21,11 +21,21 @@ namespace dynamixel_robot_gazebo ...@@ -21,11 +21,21 @@ namespace dynamixel_robot_gazebo
CComplianceControl *compliance; CComplianceControl *compliance;
unsigned char id; unsigned char id;
std::string model; std::string model;
unsigned int encoder_res;
double max_angle;
double gear_ratio;
double max_torque;
double max_speed;
unsigned char memory[SERVO_MEM_SIZE]; unsigned char memory[SERVO_MEM_SIZE];
protected: protected:
void init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh); void init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh);
void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh,double max_torque,unsigned int enc_res,double max_angle); void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
void init_memory(void); void init_memory(void);
void set_current_angle(double angle);
void set_current_speed(double speed);
void set_current_effort(double effort);
double get_target_angle(void);
double get_target_speed(void);
void on_ping(void); void on_ping(void);
unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data); unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data);
unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data); unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data);
......
src/dynamixel_servo.cpp
+ 205
− 10
View file @ 4acf7d84
...@@ -24,42 +24,92 @@ namespace dynamixel_robot_gazebo ...@@ -24,42 +24,92 @@ namespace dynamixel_robot_gazebo
} }
if(model=="AX12W") if(model=="AX12W")
{ {
this->init_compliance(joint_name,controller_nh,1.5,1024,300.0); this->encoder_res=1023;
this->max_angle=300.0;
this->gear_ratio=32.0;
this->max_torque=1.5;
this->max_speed=324.0;
this->init_compliance(joint_name,controller_nh);
} }
else if(model=="AX12A") else if(model=="AX12A")
{ {
this->init_compliance(joint_name,controller_nh,1.5,1024,300.0); this->encoder_res=1023;
this->max_angle=300.0;
this->gear_ratio=254.0;
this->max_torque=1.5;
this->max_speed=354.0;
this->init_compliance(joint_name,controller_nh);
} }
else if(model=="AX18A") else if(model=="AX18A")
{ {
this->init_compliance(joint_name,controller_nh,1.8,1024,300.0); this->encoder_res=1023;
this->max_angle=300.0;
this->gear_ratio=254.0;
this->max_torque=1.8;
this->max_speed=582.0;
this->init_compliance(joint_name,controller_nh);
} }
else if(model=="MX12W") else if(model=="MX12W")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=32.0;
this->max_torque=1.5;
this->max_speed=2820.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX28v1") else if(model=="MX28v1")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=193.0;
this->max_torque=2.5;
this->max_speed=330.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX64v1") else if(model=="MX64v1")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=200.0;
this->max_torque=6.0;
this->max_speed=378.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX106v1") else if(model=="MX106v1")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=225.0;
this->max_torque=8.4;
this->max_speed=270.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX28v2") else if(model=="MX28v2")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=193.0;
this->max_torque=2.5;
this->max_speed=330.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX64v2") else if(model=="MX64v2")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=200.0;
this->max_torque=6.0;
this->max_speed=378.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="MX106v2") else if(model=="MX106v2")
{ {
this->encoder_res=4095;
this->max_angle=360.0;
this->gear_ratio=225.0;
this->max_torque=8.4;
this->max_speed=270.0;
this->init_pid(joint_name,controller_nh); this->init_pid(joint_name,controller_nh);
} }
else if(model=="XL320") else if(model=="XL320")
...@@ -119,11 +169,11 @@ namespace dynamixel_robot_gazebo ...@@ -119,11 +169,11 @@ namespace dynamixel_robot_gazebo
} }
} }
void CDynServo::init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh,double max_torque,unsigned int enc_res,double max_angle) void CDynServo::init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh)
{ {
ros::NodeHandle params_nh(controller_nh,joint_name+std::string("/params")); ros::NodeHandle params_nh(controller_nh,joint_name+std::string("/params"));
this->compliance=new CComplianceControl(); this->compliance=new CComplianceControl();
if(!this->compliance->init(params_nh,max_torque,enc_res,max_angle)) if(!this->compliance->init(params_nh,this->max_torque,this->encoder_res,this->max_angle))
{ {
ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Complaince params from ROS parameter server."); ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Complaince params from ROS parameter server.");
delete this->compliance; delete this->compliance;
...@@ -166,7 +216,9 @@ namespace dynamixel_robot_gazebo ...@@ -166,7 +216,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=4; this->memory[26]=4;
this->memory[27]=4; this->memory[27]=4;
this->memory[28]=64; this->memory[28]=64;
this->memory[28]=64; this->memory[29]=64;
this->memory[30]=255;
this->memory[31]=1;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
this->memory[48]=32; this->memory[48]=32;
...@@ -183,7 +235,9 @@ namespace dynamixel_robot_gazebo ...@@ -183,7 +235,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=1; this->memory[26]=1;
this->memory[27]=1; this->memory[27]=1;
this->memory[28]=32; this->memory[28]=32;
this->memory[28]=32; this->memory[29]=32;
this->memory[30]=255;
this->memory[31]=1;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
this->memory[48]=32; this->memory[48]=32;
...@@ -199,7 +253,9 @@ namespace dynamixel_robot_gazebo ...@@ -199,7 +253,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=1; this->memory[26]=1;
this->memory[27]=1; this->memory[27]=1;
this->memory[28]=32; this->memory[28]=32;
this->memory[28]=32; this->memory[29]=32;
this->memory[30]=255;
this->memory[31]=1;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
this->memory[48]=32; this->memory[48]=32;
...@@ -216,6 +272,8 @@ namespace dynamixel_robot_gazebo ...@@ -216,6 +272,8 @@ namespace dynamixel_robot_gazebo
// initialize RAM // initialize RAM
this->memory[26]=8; this->memory[26]=8;
this->memory[28]=8; this->memory[28]=8;
this->memory[30]=255;
this->memory[31]=7;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
this->memory[48]=32; this->memory[48]=32;
...@@ -230,6 +288,8 @@ namespace dynamixel_robot_gazebo ...@@ -230,6 +288,8 @@ namespace dynamixel_robot_gazebo
this->memory[22]=1; this->memory[22]=1;
// initialize RAM // initialize RAM
this->memory[28]=32; this->memory[28]=32;
this->memory[30]=255;
this->memory[31]=7;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
} }
...@@ -244,6 +304,8 @@ namespace dynamixel_robot_gazebo ...@@ -244,6 +304,8 @@ namespace dynamixel_robot_gazebo
this->memory[22]=1; this->memory[22]=1;
// initialize RAM // initialize RAM
this->memory[28]=32; this->memory[28]=32;
this->memory[30]=255;
this->memory[31]=7;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
} }
...@@ -258,6 +320,8 @@ namespace dynamixel_robot_gazebo ...@@ -258,6 +320,8 @@ namespace dynamixel_robot_gazebo
this->memory[22]=1; this->memory[22]=1;
// initialize RAM // initialize RAM
this->memory[28]=32; this->memory[28]=32;
this->memory[30]=255;
this->memory[31]=7;
this->memory[34]=255; this->memory[34]=255;
this->memory[35]=3; this->memory[35]=3;
} }
...@@ -292,6 +356,8 @@ namespace dynamixel_robot_gazebo ...@@ -292,6 +356,8 @@ namespace dynamixel_robot_gazebo
this->memory[78]=100; this->memory[78]=100;
this->memory[84]=52; this->memory[84]=52;
this->memory[85]=3; this->memory[85]=3;
this->memory[116]=255;
this->memory[117]=7;
} }
else if(model=="MX64v2") else if(model=="MX64v2")
{ {
...@@ -314,6 +380,8 @@ namespace dynamixel_robot_gazebo ...@@ -314,6 +380,8 @@ namespace dynamixel_robot_gazebo
this->memory[78]=100; this->memory[78]=100;
this->memory[84]=52; this->memory[84]=52;
this->memory[85]=3; this->memory[85]=3;
this->memory[116]=255;
this->memory[117]=7;
} }
else if(model=="MX106v2") else if(model=="MX106v2")
{ {
...@@ -335,25 +403,147 @@ namespace dynamixel_robot_gazebo ...@@ -335,25 +403,147 @@ namespace dynamixel_robot_gazebo
this->memory[78]=100; this->memory[78]=100;
this->memory[84]=52; this->memory[84]=52;
this->memory[85]=3; this->memory[85]=3;
this->memory[116]=255;
this->memory[117]=7;
} }
} }
} }
void CDynServo::set_current_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")
{
this->memory[36]=value&0x000000FF;
this->memory[37]=(value>>8)&0x000000FF;
}
else
{
this->memory[132]=value&0x000000FF;
this->memory[133]=(value>>8)&0x000000FF;
this->memory[134]=(value>>16)&0x000000FF;
this->memory[135]=(value>>24)&0x000000FF;
}
}
void CDynServo::set_current_speed(double speed)
{
unsigned int value;
value=((speed*180.0)/3.14159)/6.0;//RPM
if(model=="AX12W" || model=="AX12A" || model=="AX18A")
{
value/=0.111;
this->memory[38]=value&0x000000FF;
this->memory[39]=(value>>8)&0x000000FF;
}
else if (model=="MX12W")
{
value/=0.916;
this->memory[38]=value&0x000000FF;
this->memory[39]=(value>>8)&0x000000FF;
}
else if(model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
{
value/=0.114;
this->memory[38]=value&0x000000FF;
this->memory[39]=(value>>8)&0x000000FF;
}
else
{
value/=0.229;
this->memory[128]=value&0x000000FF;
this->memory[129]=(value>>8)&0x000000FF;
this->memory[130]=(value>>16)&0x000000FF;
this->memory[131]=(value>>24)&0x000000FF;
}
}
void CDynServo::set_current_effort(double effort)
{
unsigned int value;
if(effort<0.0)
value=((fabs(effort)*1023.0)/this->max_torque)+1024;
else
value=((fabs(effort)*1023.0)/this->max_torque);
if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
{
this->memory[40]=value&0x000000FF;
this->memory[41]=(value>>8)&0x000000FF;
}
else
{
this->memory[124]=value&0x000000FF;
this->memory[125]=(value>>8)&0x000000FF;
}
}
double CDynServo::get_target_angle(void)
{
unsigned int value=0;
double angle;
if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
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);
angle=(((((value*this->max_angle)/this->encoder_res))-this->max_angle/2.0)*3.14159)/180.0;
return angle;
}
double CDynServo::get_target_speed(void)
{
unsigned int value=0;
double speed;
if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
{
this->memory[32]=value&0x000000FF;
this->memory[33]=(value>>8)&0x000000FF;
}
else
{
this->memory[104]=value&0x000000FF;
this->memory[105]=(value>>8)&0x000000FF;
this->memory[106]=(value>>16)&0x000000FF;
this->memory[107]=(value>>24)&0x000000FF;
}
if(model=="AX12W" || model=="AX12A" || model=="AX18A")
value*=0.111;
else if (model=="MX12W")
value*=0.916;
else if(model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
value*=0.114;
else
value*=0.229;
speed=((value*6.0)*3.14159)/180.0;
return speed;
}
void CDynServo::on_ping(void) void CDynServo::on_ping(void)
{ {
std::cout << "Servo " << this->id << " pinged" << std::endl; std::cout << "Servo " << (int)this->id << " pinged" << std::endl;
} }
unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data) unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
{ {
for(unsigned int i=0;i<length;i++) for(unsigned int i=0;i<length;i++)
data[i]=this->memory[address+i]; data[i]=this->memory[address+i];
return 0;
} }
unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data) unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
{ {
for(unsigned int i=0;i<length;i++) for(unsigned int i=0;i<length;i++)
this->memory[address+i]=data[i]; this->memory[address+i]=data[i];
return 0;
} }
void CDynServo::update(const ros::Duration& period) void CDynServo::update(const ros::Duration& period)
...@@ -361,9 +551,14 @@ namespace dynamixel_robot_gazebo ...@@ -361,9 +551,14 @@ namespace dynamixel_robot_gazebo
double real_angle,target_angle,command; double real_angle,target_angle,command;
real_angle=this->joint.getPosition(); real_angle=this->joint.getPosition();
target_angle=0.0;//((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0; this->set_current_angle(real_angle);
this->set_current_speed(this->joint.getVelocity());
this->set_current_effort(this->joint.getEffort());
target_angle=this->get_target_angle();
if(this->pid!=NULL) if(this->pid!=NULL)
command=this->pid->computeCommand(target_angle-real_angle,period); command=this->pid->computeCommand(target_angle-real_angle,period);
else if(this->compliance!=NULL)
command=this->compliance->control(target_angle-real_angle);
this->joint.setCommand(command); this->joint.setCommand(command);
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment