From 12890911118fdbed522a5f608c5d60f41f8bda90 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Wed, 16 Dec 2015 06:29:44 -0800 Subject: [PATCH] Updated formula for computing distance for the Sharp GP2D12 IR range sensor --- ros_arduino_python/src/ros_arduino_python/arduino_sensors.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py index 8ea69bb..d535f8d 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -22,6 +22,7 @@ import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * +from math import pow LOW = 0 HIGH = 1 @@ -210,12 +211,14 @@ class GP2D12(IRSensor): def read_value(self): value = self.controller.analog_read(self.pin) + # The GP2D12 cannot provide a meaning result closer than 3 cm. if value <= 3.0: return float('NaN') try: - distance = (6787.0 / (float(value) - 3.0)) - 4.0 + distance = pow(4187.8 / value, 1.106) + #distance = (6787.0 / (float(value) - 3.0)) - 4.0 except: return float('NaN')