Commits

iorodeo  committed 113c41f Draft

Added lowpass filter to force_source.

  • Participants
  • Parent commits c7a3d1d

Comments (0)

Files changed (6)

File software/filters/src/filters/__init__.py

 import kalman_filter
+import lowpass

File software/filters/src/filters/lowpass.py

+import math
+
+class Lowpass(object):
+
+    def __init__(self, y0=0.0,fcut=25.0):
+        self.y = y0
+        self.fcut = fcut
+
+    def update(self,x,dt):
+        tc = 1.0/(2.0*math.pi*self.fcut)
+        alpha = dt/(tc + dt)
+        self.y = alpha*x + (1.0-alpha)*self.y
+        return self.y
+
+    def getValue(self):
+        return self.y
+
+
+# -----------------------------------------------------------------------------
+if __name__ == '__main__':
+
+    import scipy
+    import matplotlib.pylab as pylab
+
+    filt = Lowpass()
+    f = 75.0
+
+    t = scipy.linspace(0,2.0/f,500)
+    dt = t[1] - t[0]
+    x = scipy.sin(2.0*scipy.pi*f*t)
+
+    y = []
+    for xval in x:
+        yval = filt.update(xval,dt)
+        y.append(yval)
+
+    y = scipy.array(y)
+
+    pylab.plot(t,x,'b')
+    pylab.plot(t,y,'r')
+    pylab.show()
+        
+
+
+
+
+
+
+
+
+
+
+

File software/force_source/manifest.xml

   <depend package="joy"/>
   <depend package="roscpp"/>
   <depend package="msg_and_srv"/>
+  <depend package="filters"/>
 
 </package>
 

File software/force_source/nodes/force_calibration.py

 roslib.load_manifest('force_source')
 import rospy
 import threading
+from filters import lowpass
 from std_msgs.msg import Header
 from msg_and_srv.msg import ForceMsg
 from msg_and_srv.msg import AnalogInMsg
         self.force_calibration = rospy.get_param("force_calibration", 1.0)
         self.force_zeroing_num = rospy.get_param("force_zeroing_num", 25)
         self.force_ain_channel = rospy.get_param("force_ain_channel" , 0)
+        filt_fcut = rospy.get_param('force_filt_cutoff', 25.0)
 
         # Set up subscriber to analog input topic
         self.ain_sub = rospy.Subscriber('analog_input', AnalogInMsg, self.ain_callback)
 
+        # Lowpass filter - added to try to deal w/ stability issue
+        self.lowpass = lowpass.Lowpass(fcut=filt_fcut)
+
         # Set up publisher on force topic
         self.force_msg = ForceMsg()
         self.force_pub = rospy.Publisher('force', ForceMsg)
             dt = t - self.t_last
             self.t_last = t
 
+            force_filt = self.lowpass.update(self.force,dt)
+
             # Create force message and publish
             self.force_msg.header.stamp = ros_time
             self.force_msg.dt = dt 
-            self.force_msg.force = self.force
+            self.force_msg.force = force_filt
+            self.force_msg.force_raw = self.force
             self.t_last = t
             self.force_pub.publish(self.force_msg)
 

File software/msg_and_srv/msg/ForceMsg.msg

 Header header
 float32 dt
 float32 force 
+float32 force_raw

File software/safety/nodes/watchdog_distance_node.py

         self.lock = threading.Lock()
         self.ready = False
         self.sleep_dt = 1.0/50.0 # Max sensor update rate in 50Hz.
+        #self.sleep_dt = 1.0/5.0 # Max sensor update rate to 5Hz -- RWW 10/05/2012
         self.last_stamp = None
         self.timeout= rospy.get_param('distance_timeout', 1.0)
         rospy.init_node('watchdog_distance')