From c510fd556cae359e1d6ecade1f7002a25db1c85a Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 8 Jul 2016 08:46:52 +0200
Subject: [PATCH] Simplified the formula to compute the voltage and digital
value of the simulated distance sensors. Inverted the output levels of the
GPIO to match the data from the real feet sensors.
---
.../include/bioloid_controller_cm510_impl.h | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index a485d50..f1e0fa4 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;
--
GitLab