Commit fdecbe26 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the velcoity control mode.

parent dec7c6aa
......@@ -88,7 +88,10 @@ unsigned int CDynamixelMotor::from_speeds(double speed)
if(speed>this->info.max_speed)
speed=this->info.max_speed;
counts=fabs(speed)/0.666;
if(this->info.ext_memory_map)
counts=speed/0.229;
else
counts=fabs(speed)/0.666;
return counts;
}
......@@ -106,7 +109,10 @@ double CDynamixelMotor::to_speeds(unsigned int counts)
{
double speed;
speed = counts*0.666; // Taken from the datasheets (sfoix)
if(this->info.ext_memory_map)
speed=counts*0.229;
else
speed=counts*0.666; // Taken from the datasheets (sfoix)
return speed;
}
......@@ -787,12 +793,21 @@ void CDynamixelMotor::set_current_limit(double current)
double CDynamixelMotor::get_speed_limit(void)
{
unsigned int data;
double speed;
this->read_register(this->registers[velocity_limit],data);
speed=this->to_speeds(data);
return speed;
}
void CDynamixelMotor::set_speed_limit(double speed)
{
unsigned int data;
data=this->from_speeds(speed);
this->write_register(this->registers[velocity_limit],data);
}
......@@ -995,27 +1010,60 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed,double curre
void CDynamixelMotor::move_relative_angle(double angle,double speed,double current)
{
unsigned int cmd[2],pos;
unsigned int data,pos;
double abs_angle;
this->set_control_mode(position_ctrl);
this->read_register(this->registers[current_pos],pos);
abs_angle=angle+this->to_angles(pos);
if(abs_angle>this->info.center_angle)
abs_angle=this->info.center_angle;
else if(abs_angle<-this->info.center_angle)
abs_angle=-this->info.center_angle;
cmd[0]=this->from_angles(abs_angle);
this->write_register(this->registers[goal_pos],cmd[0]);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
cmd[1]=this->from_speeds(speed);
this->write_register(this->registers[goal_speed],cmd[1]);
if(current==std::numeric_limits<double>::infinity())
{
this->set_control_mode(position_ctrl);
this->read_register(this->registers[current_pos],pos);
abs_angle=angle+this->to_angles(pos);
if(abs_angle>this->info.center_angle)
abs_angle=this->info.center_angle;
else if(abs_angle<-this->info.center_angle)
abs_angle=-this->info.center_angle;
data=this->from_angles(abs_angle);
this->write_register(this->registers[goal_pos],data);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
data=this->from_speeds(speed);
this->write_register(this->registers[goal_speed],data);
}
else
{
this->set_control_mode(current_pos_ctrl);
if(current>this->info.max_current)
current=this->info.max_current;
else if(current<-this->info.max_current)
current=-this->info.max_current;
data=(signed short int)(current*1000.0/2.69);
this->write_register(this->registers[goal_current],data);
this->read_register(this->registers[current_pos],pos);
abs_angle=angle+this->to_angles(pos);
if(abs_angle>this->info.center_angle)
abs_angle=this->info.center_angle;
else if(abs_angle<-this->info.center_angle)
abs_angle=-this->info.center_angle;
data=this->from_angles(abs_angle);
this->write_register(this->registers[goal_pos],data);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
data=this->from_speeds(speed);
this->write_register(this->registers[goal_speed],data);
}
}
void CDynamixelMotor::move_speed(double speed)
{
unsigned int data;
this->set_control_mode(velocity_ctrl);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
else if(speed<-this->info.max_speed)
speed=-this->info.max_speed;
data=this->from_speeds(speed);
this->write_register(this->registers[goal_speed],data);
}
void CDynamixelMotor::move_pwm(double pwm_ratio)
......@@ -1046,6 +1094,10 @@ void CDynamixelMotor::move_current(double current)
unsigned int data;
this->set_control_mode(current_ctrl);
if(current>this->info.max_current)
current=this->info.max_current;
else if(current<-this->info.max_current)
current=-this->info.max_current;
data=(signed short int)(current*1000.0/2.69);
this->write_register(this->registers[goal_current],data);
}
......@@ -1054,7 +1106,7 @@ void CDynamixelMotor::stop(void)
{
unsigned int current_position;
if(this->mode==position_ctrl)
if(this->mode==position_ctrl || this->mode==ext_position_ctrl || this->mode==current_pos_ctrl)
{
this->read_register(this->registers[current_pos],current_position);
if(this->to_angles(current_position)>this->config.max_angle)
......@@ -1063,8 +1115,12 @@ void CDynamixelMotor::stop(void)
current_position=from_angles(this->config.min_angle);
this->write_register(this->registers[goal_pos],current_position);
}
else
this->write_register(this->registers[goal_speed],0);
else if(this->mode==current_ctrl)
this->move_current(0.0);
else if(this->mode==velocity_ctrl)
this->move_speed(0.0);
else if(this->mode==pwm_ctrl)
this->move_pwm(0.0);
}
double CDynamixelMotor::get_current_angle(void)
......
Supports Markdown
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