diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp index ba4ef20dfa2c55a27c3de050b71ead95496c6edd..5b6ca9fa5831fdb9da1f68e7d2ec06d52bb23852 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;