Merge pull request ROBOTIS-GIT#57 from ROBOTIS-GIT/develop

Taehoon Lim · web-flow · commit abdf325f7c13 · 2017-07-07T13:17:23.000+09:00
bug fixed
diff --git a/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/torque_control.h b/dynamixel_workbench_controllers/include/dynamixel_workbench_controllers/torque_control.h
@@ -95,7 +95,7 @@ class TorqueControl
   int16_t convertTorque2Value(float torque);
 
   uint32_t convertRadian2Value(float radian);
-  float convertValue2Radian(uint32_t value);
+  float convertValue2Radian(int32_t value);
 
   bool gravityCompensation();
 
diff --git a/dynamixel_workbench_controllers/src/torque_control.cpp b/dynamixel_workbench_controllers/src/torque_control.cpp
@@ -258,7 +258,7 @@ uint32_t TorqueControl::convertRadian2Value(float radian)
   return value;
 }
 
-float TorqueControl::convertValue2Radian(uint32_t value)
+float TorqueControl::convertValue2Radian(int32_t value)
 {
   float radian = 0.0;
 
@@ -317,7 +317,7 @@ bool TorqueControl::gravityCompensation()
                  d_gain_ * ((error[PAN] - pre_error[PAN]) / 0.004);
   torque[TILT] = p_gain_ * error[TILT] +
                  d_gain_ * ((error[TILT] - pre_error[TILT]) / 0.004) +
-                 tilt_motor_mass * gravity * link_length * cos(convertValue2Radian(motorPos_->cur_pos.at(TILT)));
+                 tilt_motor_mass * gravity * link_length * cos(convertValue2Radian((int32_t)motorPos_->cur_pos.at(TILT)));
 
   setCurrent(convertTorque2Value(torque[PAN]), convertTorque2Value(torque[TILT]));
 

Original file line number	Diff line number	Diff line change
`@@ -258,7 +258,7 @@ uint32_t TorqueControl::convertRadian2Value(float radian)`
`258`	`258`	`return value;`
`259`	`259`	`}`
`260`	`260`
`261`		`-float TorqueControl::convertValue2Radian(uint32_t value)`
	`261`	`+float TorqueControl::convertValue2Radian(int32_t value)`
`262`	`262`	`{`
`263`	`263`	`float radian = 0.0;`
`264`	`264`
`@@ -317,7 +317,7 @@ bool TorqueControl::gravityCompensation()`
`317`	`317`	`d_gain_ * ((error[PAN] - pre_error[PAN]) / 0.004);`
`318`	`318`	`torque[TILT] = p_gain_ * error[TILT] +`
`319`	`319`	`d_gain_ * ((error[TILT] - pre_error[TILT]) / 0.004) +`
`320`		`- tilt_motor_mass * gravity * link_length * cos(convertValue2Radian(motorPos_->cur_pos.at(TILT)));`
	`320`	`+ tilt_motor_mass * gravity * link_length * cos(convertValue2Radian((int32_t)motorPos_->cur_pos.at(TILT)));`
`321`	`321`
`322`	`322`	`setCurrent(convertTorque2Value(torque[PAN]), convertTorque2Value(torque[TILT]));`
`323`	`323`