diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index a485d50662a9226713199f5a474ea18d046f5e24..f1e0fa40453b699d28861035787f04ca491f4e5f 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -432,7 +432,7 @@ namespace bioloid_controller_cm510
       {
         const double Vmax=3.3,Vmin=0.5;
         // compute the range value in volts
-        B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
+        B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-Vmin*msg->min_range);
         C=(100.0-Vmax*msg->min_range*B)/Vmax;
         voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
         value=voltage*1023.0/Vmax;// transform it to a digital value
@@ -454,7 +454,7 @@ namespace bioloid_controller_cm510
       {
         const double Vmax=5.0,Vmin=0.5;
         // compute the range value in volts
-        B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-((Vmin*Vmax*msg->min_range)/Vmax));
+        B=(100.0-(Vmin*100.0)/Vmax)/(Vmin*msg->max_range-Vmin*msg->min_range);
         C=(100.0-Vmax*msg->min_range*B)/Vmax;
         voltage=(100.0/(B*msg->range+C));// apply the approximation of the transfer function of the sensor
         value=voltage*1023.0/Vmax;// transform it to a digital value
@@ -481,7 +481,7 @@ namespace bioloid_controller_cm510
       /* handle the feet sensors */
       for(i=0;i<EXP_NUM_GPIO;i++)
       {
-        if(msg->range<(msg->max_range/2.0))
+        if(msg->range>(msg->max_range/2.0))
           threshold=0x01;
         else
           threshold=0x00;