vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/DistanceSensors.cpp
- Revision:
- 17:4e1be70bdedb
- Parent:
- 4:67d7177c213f
diff -r 915f8839fe48 -r 4e1be70bdedb Sources/DistanceSensors.cpp --- a/Sources/DistanceSensors.cpp Thu May 11 18:57:45 2017 +0000 +++ b/Sources/DistanceSensors.cpp Thu May 18 14:27:09 2017 +0000 @@ -38,7 +38,7 @@ { float Usensor; - + if (number < 6){ *bit0 = number & 1; // Set the first bit of the Sensors MUX *bit1 = number & 2; // Set the second bit of the Sensors MUX @@ -61,8 +61,15 @@ } //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor - float Distance=-0.38f*sqrt(Usensor)+0.38f; + Usensor *= 3.3f; + float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628; + Distance /= 100; + + static float distance_filtered = 0.0f; + distance_filtered = 0.55f * distance_filtered + 0.45f * Distance; + return Distance; + return distance_filtered; } DistanceSensors::operator float()