def __init__(self):
# Setup Yaw Pid
self.ypid = pypid.Pid(0.0, 0.0, 0.0)
self.ypid.set_setpoint(0.0)
#self.pid.set_inputisangle(True,pi)
self.ypid.set_derivfeedback(True) # D term in feedback look
fc = 20
# cutoff freq in hz
wc = fc * (2.0 * pi) # cutoff freq. in rad/s
self.ypid.set_derivfilter(1, wc)
self.ypid.set_maxIout(1.0)
# Setup Velocity Pid
self.vpid = pypid.Pid(0.0, 0.0, 0.0)
self.vpid.set_setpoint(0.0)
self.vpid.set_maxIout(1.0)
#self.pid.set_inputisangle(True,pi)
self.vpid.set_derivfeedback(True) # D term in feedback look
fc = 20
# cutoff freq in hz
wc = fc * (2.0 * pi) # cutoff freq. in rad/s
self.vpid.set_derivfilter(1, wc)
# Initialize some bits as none - for now
self.drivemsg = None
self.publisher = None
self.lasttime = None
# For diagnosing/tuning PID
self.vpubdebug = None
self.ypubdebug = None
def __init__(self, engaged=False, yaw_cntrl=True, vel_cntrl=True):
# Detup Yaw Pid
self.engaged = engaged
self.yaw_cntrl = yaw_cntrl
self.vel_cnrtl = vel_cntrl
self.ypid = pypid.Pid(0.0, 0.0, 0.0)
self.ypid.set_setpoint(0.0)
#self.[id.set_inputisangle(True,pi)
self.ypid.set_derivfeedback(True)
fc = 20
#cutoff freq in Hz
wc = fc * (2.0 * pi) #cutoff freq in rad/s
self.ypid.set_derivfilter(1, wc)
self.ypid.set_maxIout(1.0)
# Setup Velocity Pid
self.vpid = pypid.Pid(0.0, 0.0, 0.0)
self.vpid.set_setpoint(0.0)
self.vpid.set_maxIout(1.0)
#self.pid.set_inputisangle(Truempi)
self.vpid.set_derivfeedback(True)
fc = 20
# cutoff freq in Hz
wc = fc * (2.0 * pi) # cutoff freq in rad/s
self.vpid.set_derivfilter(1, wc)
# Initialize as none for now
self.drivemsg = None
self.publisher = None
self.lasttime = None
# For tuning PID
self.vpubdebug = None
self.ypubdebug = None
# Type of yaw control
self.yaw_type = 'yaw'
def __init__(self):
Kp = 0.0
Ki = 0.0
Kd = 0.1
self.pid = pypid.Pid(Kp, Ki, Kd)
self.pid.set_setpoint(-pi / 2)
self.pid.set_inputisangle(True, pi)
self.pid.set_derivfeedback(True) # D term in feedback look
fc = 50
# cutoff freq in hz
wc = fc * (2.0 * pi) # cutoff freq. in rad/s
self.pid.set_derivfilter(1, wc)
self.drivemsg = None
self.publisher = None
self.lasttime = None
# For diagnosing/tuning PID
self.pubdebug = None
# Simulate
tt = arange(0, 20, dt)
# pid obj
wc = 100 / (2 * pi)
# cut-off freq for filters
Kp = 100.0
Ki = 1.0
Kd = 50.0
# dict list of controllers to test
Conts = {}
# Compare standard with deriv feedback
Conts['Pstandard'] = pypid.Pid(Kp, Ki, Kd)
Conts['Pderivfeedback'] = pypid.Pid(Kp, Ki, Kd)
Conts['Pderivfeedback'].set_derivfeedback(True)
Conts['Pderivfeedbackdfilt'] = pypid.Pid(Kp, Ki, Kd)
Conts['Pderivfeedbackdfilt'].set_derivfeedback(True)
Conts['Pderivfeedbackdfilt'].set_derivfilter(2, wc)
Conts['Pderivfeedbackbothfilt'] = pypid.Pid(Kp, Ki, Kd)
Conts['Pderivfeedbackbothfilt'].set_derivfeedback(True)
Conts['Pderivfeedbackbothfilt'].set_derivfilter(2, wc)
Conts['Pderivfeedbackbothfilt'].set_inputfilter(2, wc)
'''
# Velocity Controllers
Kp = 10.0
Ki = 10.0
Kd = 1.0