From dda4370961e216093155b686d06e34cac7d4aca8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu> Date: Thu, 25 Feb 2016 09:13:43 +0000 Subject: [PATCH] Saturated the speed, angle and torque in the move_absolute(), move_relative() and move_torque() functions. --- src/dynamixel_motor.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index ba4ef20..5b6ca9f 100644 --- a/src/dynamixel_motor.cpp +++ b/src/dynamixel_motor.cpp @@ -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; -- GitLab