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

Saturated the speed, angle and torque in the move_absolute(), move_relative()...

Saturated the speed, angle and torque in the move_absolute(), move_relative() and move_torque() functions.
parent 552aca9b
No related branches found
No related tags found
No related merge requests found
......@@ -1105,7 +1105,13 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
{
try{
this->set_control_mode(angle_ctrl);
if(angle>this->info.center_angle)
angle=this->info.center_angle;
else if(angle<-this->info.center_angle)
angle=-this->info.center_angle;
cmd[0]=this->from_angles(angle);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
cmd[1]=this->from_speeds(speed);
this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4);
}catch(CDynamixelAlarmException &e){
......@@ -1120,6 +1126,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
void CDynamixelMotor::move_relative_angle(double angle,double speed)
{
unsigned short int cmd[2],pos;
double abs_angle;
if(this->dynamixel_dev==NULL)
{
......@@ -1131,7 +1138,14 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
try{
this->set_control_mode(angle_ctrl);
this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
cmd[0]=this->from_angles(angle+this->to_angles(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);
if(speed>this->info.max_speed)
speed=this->info.max_speed;
cmd[1]=this->from_speeds(speed);
this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4);
}catch(CDynamixelAlarmException &e){
......@@ -1156,6 +1170,10 @@ void CDynamixelMotor::move_torque(double torque_ratio)
{
try{
this->set_control_mode(torque_ctrl);
if(torque_ratio>100.0)
torque_ratio=100.0;
else if(torque_ratio<-100.0)
torque_ratio=-100.0;
if(torque_ratio<0.0)
torque+=0x0400;
torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment