new_method.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('cob_relayboard')
00003 import rospy
00004 import time
00005 import csv
00006 from cob_relayboard.msg import EmergencyStopState
00007 from std_msgs.msg import Float64
00008 import savizky
00009 #from pr2_msgs.msg import PowerState
00010 
00011 class volts_filter():
00012     
00013     def __init__(self):
00014         self.volts = 0.
00015         self.wsize = 61
00016         self.filter_order = 3
00017         self.theta = rospy.get_param("theta")
00018         self.off_y = rospy.get_param("off_y")
00019         self.abcd = rospy.get_param("abcd")
00020         self.hvalue = 0
00021         self.sg = savitzky.savitzky_golay(window_size=wsize, order=filter_order)
00022         self.volt_filt = None
00023         
00024         rospy.Subscriber("/power_board/voltage", Float64, callback)
00025     
00026     def callback(self, data):
00027 
00028         if(self.hvalue == 0):
00029             self.hvalue = data
00030             self.volt_filt = hvalue*np.ones(2*wsize+1)
00031             
00032             if(data <= 44000):
00033                 self.volts = 44000
00034             time_r = 0.
00035             return
00036         elif(data >= 48000):
00037                 self.volts = 48000
00038                 
00039         else:
00040             self.volts = data
00041             
00042         self.volt_filt = np.insert(self.volt_filt, 0, self.volts)
00043         self.volt_filt = np.delete(self.volt_filt, -1)
00044 
00045         vfilt = sg.filter(volt_filt)
00046         old_settings = np.seterr(all='raise')
00047         
00048         self.t_est = np.polyval(self.abcd, self.vfilt[self.wsize])
00049         
00050         self.t_est = vfilt[self.wsize]*sin(self.theta) + self.t_est*cos(self.theta)
00051 
00052         self.t_est = self.t_est + off_y
00053 
00054         
00055 if __name__ == '__main__':
00056     rospy.init_node('volt_filt')
00057         vf = volts_filter()
00058         
00059         while not rospy.is_shutdown():
00060                 rospy.sleep(1.0)
00061 
00062 


cob_relayboard
Author(s): Christian Connette
autogenerated on Sun Oct 5 2014 23:01:10