class PIDController(object):
'''
Provides a configurable PID motor controller via a threaded
fixed-time loop.
If the 'ros:motors:pid:enable_slew' property is True, this will set a slew
limiter on the velocity setter.
'''
def __init__(self,
config,
motor,
setpoint=0.0,
sample_time=0.01,
level=Level.INFO):
'''
:param config: The application configuration, read from a YAML file.
:param motor: The motor to be controlled.
:param setpoint: The initial setpoint or target output
:param sample_time: The time in seconds before generating a new
output value. This PID is expected to be called at a constant
rate.
:param level: The log level, e.g., Level.INFO.
'''
if motor is None:
raise ValueError('null motor argument.')
self._motor = motor
self._log = Logger('pidc:{}'.format(motor.orientation.label), level)
if sys.version_info < (3, 0):
self._log.error('PID class requires Python 3.')
sys.exit(1)
# PID configuration ................................
if config is None:
raise ValueError('null configuration argument.')
_config = config['ros'].get('motors').get('pid-controller')
self._pot_ctrl = _config.get('pot_ctrl')
if self._pot_ctrl:
self._pot = Potentiometer(config, Level.INFO)
self._rate = Rate(_config.get('sample_freq_hz'), level)
_sample_time = self._rate.get_period_sec()
self._pid = PID(config,
self._motor.orientation,
sample_time=_sample_time,
level=level)
self._enable_slew = True #_config.get('enable_slew')
self._slewlimiter = SlewLimiter(config,
orientation=self._motor.orientation,
level=Level.INFO)
_slew_rate = SlewRate.NORMAL # TODO _config.get('slew_rate')
self._slewlimiter.set_rate_limit(_slew_rate)
if self._enable_slew:
self._log.info('slew limiter enabled, set to {:7.4f}.'.format(
_slew_rate.limit))
self._slewlimiter.enable()
else:
self._log.info('slew limiter disabled.')
self._power = 0.0
self._last_power = 0.0
self._failures = 0
self._enabled = False
self._disabling = False
self._closed = False
self._thread = None
# self._last_steps = 0
# self._max_diff_steps = 0
self._log.info('ready.')
# ..........................................................................
def reset(self):
self._pid.velocity = 0.0
self._pid.reset()
# self._motor.reset_steps()
self._motor.stop()
self._log.info(Fore.GREEN + 'reset.')
# ..........................................................................
@property
def orientation(self):
return self._motor.orientation
# ..............................................................................
def enable_slew(self, enable):
_old_value = self._enable_slew
if not self._slewlimiter.is_enabled():
self._slewlimiter.enable()
self._enable_slew = enable
return _old_value
# ..............................................................................
def set_slew_rate(self, slew_rate):
if type(slew_rate) is SlewRate:
self._slewlimiter.set_rate_limit(slew_rate)
else:
raise Exception('expected SlewRate argument, not {}'.format(
type(slew_rate)))
# ..............................................................................
@property
def steps(self):
return self._motor.steps
def reset_steps(self):
self._motor._steps = 0
# ..........................................................................
@property
def velocity(self):
'''
Getter for the velocity (PID set point).
'''
return self._pid.velocity
@velocity.setter
def velocity(self, velocity):
'''
Setter for the velocity (PID set point).
'''
if self._enable_slew:
velocity = self._slewlimiter.slew(self.velocity, velocity)
self._pid.velocity = velocity
# ..........................................................................
def set_max_velocity(self, max_velocity):
'''
Setter for the maximum velocity of both PID controls. Set
to None (the default) to disable this feature.
'''
if max_velocity == None:
self._log.info(Fore.YELLOW + 'max velocity: NONE')
else:
self._log.info(Fore.YELLOW +
'max velocity: {:5.2f}'.format(max_velocity))
self._pid.set_max_velocity(max_velocity)
# ..........................................................................
@property
def stats(self):
'''
Returns statistics a tuple for this PID contrroller:
[kp, ki, kd, cp, ci, cd, last_power, current_motor_power, power, current_velocity, setpoint, steps]
'''
kp, ki, kd = self._pid.constants
cp, ci, cd = self._pid.components
return kp, ki, kd, cp, ci, cd, self._last_power, self._motor.get_current_power_level(
), self._power, self._motor.velocity, self._pid.setpoint, self._motor.steps
# ..........................................................................
def _loop(self):
'''
The PID loop that continues while the enabled flag is True.
'''
_doc = Documentation.NONE
_power_color = Fore.BLACK
_lim = 0.2
_last_velocity = 0.0
_counter = itertools.count()
while self._enabled:
_count = next(_counter)
_current_power = self._motor.get_current_power_level()
_current_velocity = self._motor.velocity
_vpr = round(_current_velocity /
_current_power) if _current_power > 0 else -1.0
if self._pot_ctrl:
_pot_value = self._pot.get_scaled_value()
self._pid.kd = _pot_value
self._log.debug('pot: {:8.5f}.'.format(_pot_value))
if _vpr < 0.0:
self._failures += 1
elif self._failures > 0:
self._failures -= 1
self._power += self._pid(_current_velocity) #/ 100.0
self._motor.set_motor_power(self._power / 100.0)
if _doc is Documentation.MINIMAL:
self._log.info('velocity: {:>5.2f}\t{:<5.2f}'.format(
_current_velocity, self._pid.setpoint) + Style.RESET_ALL)
elif _doc is Documentation.SIMPLE:
if (_last_velocity * 0.97) <= _current_velocity <= (
_last_velocity * 1.03): # within 3%
_velocity_color = Fore.YELLOW + Style.NORMAL
elif _last_velocity < _current_velocity:
_velocity_color = Fore.MAGENTA + Style.NORMAL
elif _last_velocity > _current_velocity:
_velocity_color = Fore.CYAN + Style.NORMAL
else:
_velocity_color = Fore.BLACK
self._log.info('velocity:\t' + _velocity_color +
'{:>5.2f}\t{:<5.2f}'.format(
_current_velocity, self._pid.setpoint) +
Style.RESET_ALL)
elif _doc is Documentation.FULL:
if self._last_power < self._power:
_power_color = Fore.GREEN + Style.BRIGHT
elif self._last_power > self._power:
_power_color = Fore.RED
else:
_power_color = Fore.YELLOW
if (_last_velocity * 0.97) <= _current_velocity <= (
_last_velocity * 1.03): # within 3%
_velocity_color = Fore.YELLOW + Style.NORMAL
elif _last_velocity < _current_velocity:
_velocity_color = Fore.MAGENTA + Style.NORMAL
elif _last_velocity > _current_velocity:
_velocity_color = Fore.CYAN + Style.NORMAL
else:
_velocity_color = Fore.BLACK
kp, ki, kd = self._pid.constants
cp, ci, cd = self._pid.components
self._log.info(Fore.BLUE + 'cp={:7.4f}|ci={:7.4f}|cd={:7.4f}'.format(cp, ci, cd)\
+ Fore.CYAN + '|kd={:<7.4f} '.format(kd) \
+ _power_color + '\tpwr: {:>5.2f}/{:<5.2f}'.format(_current_power, self._power)\
+ _velocity_color + '\tvel: {:>5.2f}/{:<5.2f}'.format(_current_velocity, self._pid.setpoint)\
# + Fore.CYAN + Style.NORMAL + ' \tvp ratio: {:>5.2f}%; {:d} fails;'.format(_vpr, self._failures)\
+ Style.RESET_ALL)
# + '\t{:d}/{:d} steps.'.format(_diff_steps, self._max_diff_steps) + Style.RESET_ALL)
elif _doc is Documentation.CSV:
kp, ki, kd = self._pid.constants
cp, ci, cd = self._pid.components
self._log.info('{:7.4f}|{:7.4f}|{:7.4f}'.format(kp, ki, kd)\
+ '|{:7.4f}|{:7.4f}|{:7.4f}'.format(cp, ci, cd) \
+ '|{:>5.2f}|{:<5.2f}'.format(_current_power, self._power)\
+ '|{:>5.2f}|{:<5.2f}'.format(_current_velocity, self._pid.setpoint))
self._last_power = self._power
_last_velocity = _current_velocity
self._rate.wait() # 20Hz
# ......................
self._log.info('exited PID loop.')
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._closed:
if self._enabled:
self._log.warning('PID loop already enabled.')
return
self._log.info('enabling PID loop...')
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=PIDController._loop,
args=[self])
self._thread.start()
self._log.info(Style.BRIGHT + 'PID loop enabled.')
else:
self._log.warning(
'cannot enable PID loop: thread already exists.')
else:
self._log.warning('cannot enable PID loop: already closed.')
# ..........................................................................
def disable(self):
if self._disabling:
self._log.warning('already disabling.')
return
elif self._closed:
self._log.warning('already closed.')
return
self._disabling = True
time.sleep(0.06) # just a bit more than 1 loop in 20Hz
if self._thread is not None:
self._thread.join(timeout=1.0)
self._log.debug('pid thread joined.')
self._thread = None
self._disabling = False
self._enabled = False
self._log.info('disabled.')
# ..........................................................................
def close(self):
if self._enabled:
self.disable()
self._closed = True
# self.close_filewriter()
self._log.info('closed.')
def main():
_log = Logger('main', Level.INFO)
_rate = Rate(20) # Hz
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_pot = Potentiometer(_config, Level.INFO)
# _pot.set_out_max(3.0)
try:
# motors ...............................................................
_motors = Motors(_config, None, Level.INFO)
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
# _motor = _stbd_motor
# pid ..................................................................
_pid_config = _config['ros'].get('motors').get('pid')
_target_velocity = 0.0
_port_pid = PID(_config, setpoint=_target_velocity, sample_time=_rate.get_period_sec(), level=Level.INFO)
_stbd_pid = PID(_config, setpoint=_target_velocity, sample_time=_rate.get_period_sec(), level=Level.INFO)
# _stbd_pid.auto_mode = True
# _stbd_pid.proportional_on_measurement = True
# _stbd_pid.set_auto_mode(False)
# _stbd_pid.tunings = ( _kp, _ki, _kd )
# _stbd_pid.output_limits = ( -10.0, 10.0 )
# _stbd_pid.sample_time = _rate.get_period_sec()
# _log.info(Fore.GREEN + 'sample time: {:>6.3f} sec.'.format(_stbd_pid.sample_time))
# _clip_max = 1.0
# _clip_min = -1.0 * _clip_max
# _clip = lambda n: _clip_min if n <= _clip_min else _clip_max if n >= _clip_max else n
# _limiter = SlewLimiter(_config, None, Level.WARN)
# _limiter.enable()
# _stbd_pid.setpoint = _target_velocity
_kp = 0.0
_ki = 0.0
_kd = 0.0
_power = 0.0
_limit = -1
_count = -1
try:
while True:
_count = 0
_counter = itertools.count()
while _limit < 0 or _count < _limit:
_count = next(_counter)
_pot_value = _pot.get_scaled_value()
_target_velocity = _pot_value
_port_pid.setpoint = _target_velocity
# _port_pid.tunings = ( _kp, _ki, _kd )
_last_power = _port_motor.get_current_power_level()
_current_velocity = _port_motor.get_velocity()
_power += _port_pid(_current_velocity) / 100.0
_set_power = _power / 100.0
_port_motor.set_motor_power(_power)
# .........................
kp, ki, kd = _port_pid.constants
print(Fore.RED + '{:d}\tPID:{:6.3f}|{:6.3f}|{:6.3f};\tLAST={:>7.4f}\tSET={:>7.4f}; \tvel: {:>6.3f}/{:>6.3f}'.format(\
_count, kp, ki, kd, _last_power, _power, _current_velocity, _target_velocity) + Style.RESET_ALL)
_stbd_pid.setpoint = _target_velocity
# _stbd_pid.tunings = ( _kp, _ki, _kd )
_last_power = _stbd_motor.get_current_power_level()
_current_velocity = _stbd_motor.get_velocity()
_power += _stbd_pid(_current_velocity) / 100.0
# _set_power = _power / 100.0
_stbd_motor.set_motor_power(_power)
# .........................
kp, ki, kd = _stbd_pid.constants
print(Fore.GREEN + '{:d}\tPID:{:6.3f}|{:6.3f}|{:6.3f};\tLAST={:>7.4f}\tSET={:>7.4f}; \tvel: {:>6.3f}/{:>6.3f}'.format(\
_count, kp, ki, kd, _last_power, _power, _current_velocity, _target_velocity) + Style.RESET_ALL)
_rate.wait()
_motors.brake()
time.sleep(1.0)
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'B. motor test complete.')
finally:
_motors.brake()
'''
Kpro = 0.380
Kdrv = 0.0
last_proportional_error = 0.0
power = 0.0
while True:
Kdrv = _pot.get_scaled_value()
_current_velocity = _motor.get_velocity()
proportional_error = _target_velocity - _current_velocity
derivative_error = proportional_error - last_proportional_error
last_proportional_error = proportional_error
power += (proportional_error * Kpro) + (derivative_error * Kdrv)
_motor.set_motor_power( power / 100.0 )
_log.info(Fore.GREEN + ' P:{:6.3f}; D:{:6.3f};'.format(Kpro, Kdrv) \
+ Fore.YELLOW + ' power: {:>8.5f};'.format(power) \
+ Fore.MAGENTA + ' velocity: {:>6.3f}/{:>6.3f}'.format(_current_velocity, _target_velocity))
_rate.wait()
'''
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}'.format(e))
traceback.print_exc(file=sys.stdout)
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
class PID(object):
'''
Provides a configurable PID motor controller via a threaded
fixed-time loop.
If the 'ros:motors:pid:enable_slew' property is True, this will set a slew
limiter on the velocity setter.
'''
def __init__(self,
config,
motor,
setpoint=0.0,
sample_time=0.01,
level=Level.INFO):
'''
:param config: The application configuration, read from a YAML file.
:param motor: The motor to be controlled.
:param setpoint: The initial setpoint or target output
:param sample_time: The time in seconds before generating a new
output value. This PID is expected to be called at a constant
rate.
:param level: The log level, e.g., Level.INFO.
'''
if motor is None:
raise ValueError('null motor argument.')
self._motor = motor
self._log = Logger('pid:{}'.format(motor.get_orientation().label),
level)
if sys.version_info < (3, 0):
self._log.error('PID class requires Python 3.')
sys.exit(1)
# PID configuration ................................
if config is None:
raise ValueError('null configuration argument.')
_config = config['ros'].get('motors').get('pid')
self.kp = _config.get('kp') # proportional gain
self.ki = _config.get('ki') # integral gain
self.kd = _config.get('kd') # derivative gain
self._min_output = _config.get('min_output')
self._max_output = _config.get('max_output')
self._setpoint = setpoint
self._log.info(
'kp:{:7.4f}; ki:{:7.4f}; kd:{:7.4f};\tmin={:>5.2f}; max={:>5.2f}'.
format(self._kp, self.ki, self.kd, self._min_output,
self._max_output))
if sample_time is None:
raise Exception('no sample time argument provided')
self._rate = Rate(_config.get('sample_freq_hz'), level)
self._sample_time = self._rate.get_period_sec()
self._log.info('sample time: {:7.4f} sec'.format(self._sample_time))
self._current_time = time.monotonic # to ensure time deltas are always positive
self.reset()
# Velocity.CONFIG.configure(_config)
self._enable_slew = _config.get('enable_slew')
if self._enable_slew:
self._slewlimiter = SlewLimiter(config,
self._motor.get_orientation(),
Level.INFO)
_slew_rate = SlewRate.NORMAL # TODO _config.get('slew_rate')
self._slewlimiter.set_rate_limit(_slew_rate)
self._failures = 0
self._enabled = False
self._closed = False
self._thread = None
# self._last_steps = 0
# self._max_diff_steps = 0
self._log.info('ready.')
# ..........................................................................
@property
def kp(self):
return self._kp
@kp.setter
def kp(self, kp):
self._kp = kp
# ..........................................................................
@property
def ki(self):
return self._ki
@ki.setter
def ki(self, ki):
self._ki = ki
# ..........................................................................
@property
def kd(self):
return self._kd
@kd.setter
def kd(self, kd):
self._kd = kd
# ..........................................................................
@property
def velocity(self):
'''
Getter for the velocity (PID set point).
'''
return self.setpoint
@velocity.setter
def velocity(self, velocity):
'''
Setter for the velocity (PID set point).
'''
if self._enable_slew:
velocity = self._slewlimiter.slew(self.velocity, velocity)
self._setpoint = velocity
# ..........................................................................
@property
def setpoint(self):
'''
The setpoint used by the controller as a tuple: (Kp, Ki, Kd)
'''
return self._setpoint
@setpoint.setter
def setpoint(self, setpoint):
'''
Setter for the set point.
'''
self._setpoint = setpoint
# ..........................................................................
def __call__(self, target, dt=None):
'''
Call the PID controller with *target* and calculate and return a
control output if sample_time seconds has passed since the last
update. If no new output is calculated, return the previous output
instead (or None if no value has been calculated yet).
:param dt: If set, uses this value for timestep instead of real
time. This can be used in simulations when simulation time
is different from real time.
'''
_now = self._current_time()
if dt is None:
dt = _now - self._last_time if _now - self._last_time else 1e-16
elif dt <= 0:
raise ValueError(
"dt has nonpositive value {}. Must be positive.".format(dt))
if dt < self._sample_time and self._last_output is not None:
# only update every sample_time seconds
return self._last_output
# compute error terms
error = self._setpoint - target
d_input = target - (self._last_input
if self._last_input is not None else target)
# compute the proportional term
self._proportional = self.kp * error
# compute integral and derivative terms
self._integral += self.ki * error * dt
self._integral = self.clamp(self._integral) # avoid integral windup
self._derivative = -self.kd * d_input / dt
# compute output
output = self._proportional + self._integral + self._derivative
output = self.clamp(output)
# self._log.info(Fore.CYAN + 'error={:6.3f};'.format(error) \
# + Style.BRIGHT + ' P={:6.3f}; I={:6.3f}; D={:6.3f}; setpoint={:6.3f};'.format(self.kp, self.ki, self.kd, self._setpoint) \
# + Style.DIM + ' output: {:>6.3f};'.format(output))
# keep track of state
self._last_output = output
self._last_input = target
self._last_time = _now
return output
# ..........................................................................
@property
def sample_time(self):
'''
Return the sample time as a property.
'''
return self._sample_time
# ..........................................................................
@property
def constants(self):
'''
The P-, I- and D- fixed terms, as a tuple.
'''
return self.kp, self.ki, self.kd
# ..........................................................................
@property
def components(self):
'''
The P-, I- and D-terms from the last computation as separate
components as a tuple. Useful for visualizing what the controller
is doing or when tuning hard-to-tune systems.
'''
return self._proportional, self._integral, self._derivative
# ..........................................................................
@property
def tunings(self):
'''
The tunings used by the controller as a tuple: (Kp, Ki, Kd)
'''
return self.kp, self.ki, self.kd
# ..........................................................................
@tunings.setter
def tunings(self, tunings):
'''
Setter for the PID tunings.
'''
self._kp, self._ki, self._kd = tunings
# ..........................................................................
@property
def output_limits(self):
'''
The curre_pom output limits as a 2-tuple: (lower, upper). See also
the *output_limts* parameter in :meth:`PID.__init__`.
'''
return self._min_output, self._max_output
# ..........................................................................
@output_limits.setter
def output_limits(self, limits):
'''
Setter for the output limits.
'''
if limits is None:
self._min_output, self._max_output = None, None
return
min_output, max_output = limits
if None not in limits and max_output < min_output:
raise ValueError('lower limit must be less than upper limit')
self._min_output = min_output
self._max_output = max_output
self._integral = self.clamp(self._integral)
self._last_output = self.clamp(self._last_output)
# ..........................................................................
def reset(self):
'''
Reset the PID controller internals, setting each term to 0 as well
as cleaning the integral, the last output and the last input
(derivative calculation).
'''
self._failures = 0
self._proportional = 0.0
self._integral = 0.0
self._derivative = 0.0
self._last_time = self._current_time()
self._last_output = None
self._last_input = None
# ..........................................................................
def clamp(self, value):
'''
Clamp the value between the lower and upper limits.
'''
if value is None:
return None
elif self._max_output is not None and value > self._max_output:
return self._max_output
elif self._min_output is not None and value < self._min_output:
return self._min_output
else:
return value
# ..........................................................................
def _loop(self):
'''
The PID loop that continues while the enabled flag is True.
'''
_doc = Documentation.FULL
_power_color = Fore.BLACK
_lim = 0.2
_power = 0.0
_last_power = 0.0
_last_velocity = 0.0
_counter = itertools.count()
if self._enable_slew:
if not self._slewlimiter.is_enabled():
self._log.info('slew enabled.')
self._slewlimiter.enable()
else:
self._log.info('slew disabled.')
while self._enabled:
_count = next(_counter)
_current_power = self._motor.get_current_power_level()
_current_velocity = self._motor.velocity
_vpr = round(_current_velocity /
_current_power) if _current_power > 0 else -1.0
if _vpr < 0.0:
self._failures += 1
elif self._failures > 0:
self._failures -= 1
_power += self(_current_velocity) #/ 100.0
self._motor.set_motor_power(_power / 100.0)
if _doc is Documentation.MINIMAL:
print('velocity: {:>5.2f}\t{:<5.2f}'.format(
_current_velocity, self.setpoint) + Style.RESET_ALL)
elif _doc is Documentation.SIMPLE:
if (_last_velocity * 0.97) <= _current_velocity <= (
_last_velocity * 1.03): # within 3%
_velocity_color = Fore.YELLOW + Style.NORMAL
elif _last_velocity < _current_velocity:
_velocity_color = Fore.MAGENTA + Style.NORMAL
elif _last_velocity > _current_velocity:
_velocity_color = Fore.CYAN + Style.NORMAL
else:
_velocity_color = Fore.BLACK
print('velocity:\t' +
_velocity_color + '{:>5.2f}\t{:<5.2f}'.format(
_current_velocity, self.setpoint) + Style.RESET_ALL)
elif _doc is Documentation.FULL:
if _last_power < _power:
_power_color = Fore.GREEN + Style.BRIGHT
elif _last_power > _power:
_power_color = Fore.RED
else:
_power_color = Fore.YELLOW
if (_last_velocity * 0.97) <= _current_velocity <= (
_last_velocity * 1.03): # within 3%
_velocity_color = Fore.YELLOW + Style.NORMAL
elif _last_velocity < _current_velocity:
_velocity_color = Fore.MAGENTA + Style.NORMAL
elif _last_velocity > _current_velocity:
_velocity_color = Fore.CYAN + Style.NORMAL
else:
_velocity_color = Fore.BLACK
kp, ki, kd = self.constants
print(Fore.BLUE + '{:7.4f}|{:7.4f}|{:7.4f};'.format(kp, ki, kd)\
+ _power_color + '\tp: {:>5.2f}/{:<5.2f};'.format(_current_power, _power)\
+ Fore.CYAN + Style.NORMAL + ' \tvp ratio: {:>5.2f}%; {:d} fails;'.format(_vpr, self._failures)\
+ _velocity_color + '\tv: {:>5.2f}/{:<5.2f}'.format(_current_velocity, self.setpoint) + Style.RESET_ALL)
# + '\t{:d}/{:d} steps.'.format(_diff_steps, self._max_diff_steps) + Style.RESET_ALL)
_last_power = _power
_last_velocity = _current_velocity
self._rate.wait() # 20Hz
# ......................
self._log.info('exited PID loop.')
# ..........................................................................
def enable(self):
if not self._closed:
self._enabled = True
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._thread = threading.Thread(target=PID._loop, args=[self])
self._thread.start()
self._log.info('PID loop enabled.')
else:
self._log.warning(
'cannot enable PID loop: thread already exists.')
else:
self._log.warning('cannot enable PID loop: already closed.')
# ..........................................................................
def disable(self):
self._enabled = False
time.sleep(0.06) # just a bit more than 1 loop in 20Hz
if self._thread is not None:
self._thread.join(timeout=1.0)
self._log.debug('pid thread joined.')
self._thread = None
self._log.info('PID loop disabled.')
# ..........................................................................
def close(self):
self.close_filewriter()
self._log.info('closed.')