Commit c510fd55 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent d1d41088
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment