class BNO055:
'''
Reads from a BNO055 9DoF sensor. In order for this to be used by a
Raspberry Pi you must in theory slow your I2C bus down to around
10kHz, though if not things still seem to work. YMMV.
Note that this has been calibrated based on the orientation of the
BNO055 board as installed on the KR01 robot, with the dot on the
chip (mounted horizontally) as follows:
Fore
--------------
| • |
| |
| |
Port | | Starboard
| |
--------------
Aft
If you mount the chip in a different orientation you will likely
need to multiply one or more of the axis by -1.0.
'''
def __init__(self, config, queue, level):
self._log = Logger("bno055", level)
if config is None:
raise ValueError("no configuration provided.")
self._queue = queue
self._config = config
# config
_config = self._config['ros'].get('bno055')
self._loop_delay_sec = _config.get('loop_delay_sec')
_i2c_device = _config.get('i2c_device')
self._pitch_trim = _config.get('pitch_trim')
self._roll_trim = _config.get('roll_trim')
self._heading_trim = _config.get('heading_trim')
self._error_range = 5.0 # permitted error between Euler and Quaternion (in degrees) to allow setting value, was 3.0
self._rate = Rate(_config.get('sample_rate')) # e.g., 20Hz
# use `ls /dev/i2c*` to find out what i2c devices are connected
self._bno055 = adafruit_bno055.BNO055_I2C(
I2C(_i2c_device)) # Device is /dev/i2c-1
# some of the other modes provide accurate compass calibration but are very slow to calibrate
# self._bno055.mode = BNO055Mode.COMPASS_MODE.mode
# self._bno055.mode = BNO055Mode.NDOF_FMC_OFF_MODE.mode
self._counter = itertools.count()
self._thread = None
self._enabled = False
self._closed = False
self._heading = 0.0 # we jauntily default at zero so we don't return a None
self._pitch = None
self._roll = None
self._is_calibrated = Calibration.NEVER
self._log.info('ready.')
# ..........................................................................
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=BNO055._read,
args=[self])
self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('thread already exists.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def get_heading(self):
'''
Returns a tuple containing a Calibration (enum) indicating if the
sensor is calibrated followed by the heading value.
'''
return (self._is_calibrated, self._heading)
# ..........................................................................
def get_pitch(self):
return self._pitch
# ..........................................................................
def get_roll(self):
return self._roll
# ..........................................................................
def is_calibrated(self):
return self._is_calibrated
# ..........................................................................
def _read(self):
'''
The function that reads sensor values in a loop. This checks to see
if the 'sys' calibration is at least 3 (True), and if the Euler and
Quaternion values are within an error range of each other, this sets
the class variable for heading, pitch and roll. If they aren't within
range for more than n polls, the values revert to None.
'''
self._log.info('starting sensor read...')
_e_heading, _e_pitch, _e_roll, _e_yaw, _q_heading, _q_pitch, _q_roll, _q_yaw = [
None
] * 8
_quat_w = 0
_quat_x = 0
_quat_y = 0
_quat_z = 0
_count = 0
# begin loop .......................................
while self._enabled and not self._closed:
_status = self._bno055.calibration_status
# status: sys, gyro, accel, mag
_sys_calibrated = (_status[0] >= 3)
_gyr_calibrated = (_status[1] >= 3)
_acc_calibrated = (_status[2] >= 2)
_mag_calibrated = (_status[3] >= 2)
# "Therefore we recommend that as long as Magnetometer is 3/3, and Gyroscope is 3/3, the data can be trusted."
# source: https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BNO055-Calibration-Staus-not-stable/td-p/8375
_ok_calibrated = _gyr_calibrated and _mag_calibrated
# show calibration of each sensor
self._log.info( Fore.MAGENTA + ( Style.BRIGHT if _sys_calibrated else Style.DIM ) + ' s{}'.format(_status[0]) \
+ Fore.YELLOW + ( Style.BRIGHT if _gyr_calibrated else Style.DIM ) + ' g{}'.format(_status[1]) \
+ Fore.GREEN + ( Style.BRIGHT if _acc_calibrated else Style.DIM ) + ' a{}'.format(_status[2]) \
+ Fore.CYAN + ( Style.BRIGHT if _mag_calibrated else Style.DIM ) + ' m{}'.format(_status[3]) )
# BNO055 Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ..............
_quat = self._bno055.quaternion
if _quat != None:
_quat_w = _quat[0] if _quat[0] != None else None
_quat_x = _quat[1] if _quat[1] != None else None
_quat_y = _quat[2] if _quat[2] != None else None
_quat_z = _quat[3] if _quat[3] != None else None
if _quat_w != None \
and _quat_x != None \
and _quat_y != None \
and _quat_z != None:
_q = Quaternion(_quat_w, _quat_x, _quat_y, _quat_z)
_q_heading = _q.degrees
_q_yaw_pitch_roll = _q.yaw_pitch_roll
_q_yaw = _q_yaw_pitch_roll[0]
_q_pitch = _q_yaw_pitch_roll[1]
_q_roll = _q_yaw_pitch_roll[2]
time.sleep(0.5) # TEMP
# heading ............................................................
self._log.debug(
Fore.BLUE + Style.NORMAL +
'_quat_w={:>5.4f}, _quat_x={:>5.4f}, _quat_y={:>5.4f}, _quat_z={:>5.4f}'
.format(_quat_w, _quat_x, _quat_y, _quat_z))
if _ok_calibrated:
self._heading = _q_heading + self._heading_trim
self._log.info(Fore.MAGENTA + 'heading={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw) \
+ Fore.CYAN + Style.DIM + '\tquat: {}'.format(_quat))
self._is_calibrated = Calibration.CALIBRATED
else:
self._log.info(Fore.CYAN + Style.DIM + 'heading={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw) \
+ Fore.CYAN + Style.DIM + '\tquat: {}'.format(_quat))
self._is_calibrated = Calibration.LOST
# # # pitch ..............................................................
# # if Convert.in_range(_e_pitch, _q_pitch, self._error_range):
# # self._pitch = _e_pitch + self._pitch_trim
# # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (calibrated)'.format(_e_pitch))
# # else:
# # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'pitch: \t{:>5.4f}° (euler)'.format(_e_pitch))
# # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (quaternion)'.format(_q_pitch))
# # self._pitch = None
# # # roll ...............................................................
# # if Convert.in_range(_e_roll, _q_roll, self._error_range):
# # self._roll = _e_roll + self._roll_trim
# # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'roll: \t{:>5.4f}° (calibrated)'.format(_e_roll))
# # else:
# # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'roll: \t{:>5.4f}° (euler)'.format(_e_roll))
# # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'roll: \t{:>5.4f}° (quaternion)'.format(_q_roll))
# # self._roll = None
else:
pass
_count = next(self._counter)
self._rate.wait()
# end loop .....................................
self._log.debug('read loop ended.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._log.info('disabled.')
else:
self._log.warning('already disabled.')
# ..........................................................................
def close(self):
if not self._closed:
if self._enabled:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
def test_motors():
_log = Logger('test', Level.INFO)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
# rotary controller
_min = 0
_max = 50
_step = 5
_rotary_ctrl = RotaryControl(_config, _min, _max, _step, Level.WARN)
_motor_configurer = MotorConfigurer(_config, Level.INFO)
_motors = _motor_configurer.configure()
_motors.enable()
_log.info('starting motors...')
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid = PIDController(_config, _port_motor, level=Level.INFO)
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_motor.enable()
_port_pid.enable()
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid = PIDController(_config, _stbd_motor, level=Level.INFO)
_stbd_motor.enable()
_stbd_pid.enable()
_power = 0.0
_pot = Potentiometer(_config, Level.WARN)
if POT_CONTROL:
_pot_limit = 1.0
else:
_pot_limit = 0.0
_pot.set_output_limits(0.0, _pot_limit)
try:
_rate = Rate(10)
i = 0
# for i in range(7):
while True:
_rot_value = _rotary_ctrl.read()
# _unscaled_value = _pot.get_value()
# _pot.set_rgb(_unscaled_value)
_scaled_value = _pot.get_scaled_value(True)
_log.info('rotary : {:<5.2f};'.format(_rot_value) +
' pot: {:<5.2f}'.format(_scaled_value))
if POT_CONTROL:
_power = _scaled_value
# _power = i * 0.1
_log.info(Fore.YELLOW +
'set motor power: {:5.2f} (original: {:d});'.format(
_power, i))
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_motor.set_motor_power(-1.0 * _power)
_stbd_motor.set_motor_power(_power)
_log.info('[{:d}]\t'.format(i) \
+ Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f};\t'.format(\
_stbd_motor.get_current_power_level(), _power, _port_pid.steps, _port_pid.setpoint, _port_pid.velocity) \
+ Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f}'.format(\
_port_motor.get_current_power_level(), _power, _stbd_pid.steps, _stbd_pid.setpoint, _stbd_pid.velocity))
else:
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid.setpoint = _rot_value
_stbd_pid.setpoint = _rot_value
# _log.info(Fore.RED + 'STBD value: {:5.2f} setpoint: {:5.2f}'.format(_scaled_value, _stbd_pid.setpoint) + Style.RESET_ALL)
_log.info('[{:d}]\t'.format(i) \
+ Fore.RED + 'motor power {:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f};\t'.format(\
_stbd_motor.get_current_power_level(), _port_pid.steps, _port_pid.setpoint, _port_pid.velocity) \
+ Fore.GREEN + 'motor power {:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f}'.format(\
_port_motor.get_current_power_level(), _stbd_pid.steps, _stbd_pid.setpoint, _stbd_pid.velocity))
# if i > 2 and ( _port_motor.steps == 0 or _stbd_motor.steps == 0 ):
# raise Exception('motors are not turning.')
# time.sleep(1.0)
# time.sleep(1.0)
_rate.wait()
# tests...
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_log.info('port motor: {:d} steps.'.format(_port_pid.steps))
# assert _port_motor.steps > 0
_log.info('stbd motor: {:d} steps.'.format(_stbd_pid.steps))
# assert _stbd_motor.steps > 0
except Exception as e:
_log.error('error: {}'.format(e))
finally:
close_motors(_log, _port_motor, _stbd_motor)
_log.info(Fore.YELLOW + 'complete.')
class PID():
'''
A PID controller seeks to keep some input variable close to a desired
setpoint by adjusting an output. The way in which it does this can be
'tuned' by adjusting three parameters (P,I,D).
This uses the ros:motors:pid: and ros:geometry: sections of the
YAML configuration.
If a FileWriter is set odometry data will be written to a data file.
Various formulae are found at the bottom of this file.
'''
def __init__(self, config, motor, level):
self._motor = motor
self._orientation = motor.get_orientation()
self._log = Logger('pid:{}'.format(self._orientation.label), level)
# PID configuration ................................
if config is None:
raise ValueError('null configuration argument.')
_config = config['ros'].get('motors').get('pid')
self._enable_slew = _config.get('enable_slew')
if self._enable_slew:
self._slewlimiter = SlewLimiter(config, self._orientation, level)
# geometry configuration ...........................
_geo_config = config['ros'].get('geometry')
self._wheel_diameter = _geo_config.get('wheel_diameter')
self._wheelbase = _geo_config.get('wheelbase')
self._step_per_rotation = _geo_config.get('steps_per_rotation')
_clip_max = _config.get('clip_limit')
_clip_min = -1.0 * _clip_max
self._clip = lambda n: _clip_min if n <= _clip_min else _clip_max if n >= _clip_max else n
self._enable_p = _config.get('enable_p')
self._enable_i = _config.get('enable_i')
self._enable_d = _config.get('enable_d')
_kp = _config.get('kp')
_ki = _config.get('ki')
_kd = _config.get('kd')
self.set_tuning(_kp, _ki, _kd)
self._read_p_from_pot = _config.get('read_p_from_pot')
self._read_i_from_pot = _config.get('read_i_from_pot')
self._read_d_from_pot = _config.get('read_d_from_pot')
if self._read_p_from_pot or self._read_i_from_pot or self._read_d_from_pot:
self._pot = Potentiometer(config, level)
# set scaled output maximum value to double k*
if self._read_p_from_pot:
_out_max = 0.01500
self._log.info(Fore.YELLOW +
'scaled output maximum set from KP: {:9.6f}'.
format(_out_max))
self._pot.set_out_max(_out_max)
elif self._read_i_from_pot:
_out_max = 0.000470
self._log.info(Fore.YELLOW +
'scaled output maximum set from KI: {:9.6f}'.
format(_out_max))
self._pot.set_out_max(_out_max)
elif self._read_d_from_pot:
_out_max = 0.00200
self._log.info(Fore.YELLOW +
'scaled output maximum set from KD: {:9.6f}'.
format(_out_max))
self._pot.set_out_max(_out_max)
self._rate = Rate(_config.get('sample_freq_hz'),
level) # default sample rate is 20Hz
# self._millis = lambda: int(round(time.time() * 1000))
self._counter = itertools.count()
self._last_error = 0.0
self._sum_errors = 0.0
self._step_limit = 0
self._stats_queue = None
self._filewriter = None # optional: for statistics
self._filewriter_closed = False
_wheel_circumference = math.pi * self._wheel_diameter # mm
self._steps_per_m = 1000.0 * self._step_per_rotation / _wheel_circumference
self._log.info(Fore.BLUE + Style.BRIGHT +
'{:5.2f} steps per m.'.format(self._steps_per_m))
self._steps_per_cm = 10.0 * self._step_per_rotation / _wheel_circumference
self._log.info(Fore.BLUE + Style.BRIGHT +
'{:5.2f} steps per cm.'.format(self._steps_per_cm))
self._last_steps = 0
self._max_diff_steps = 0
self._enabled = False
self._closed = False
self._thread = None
self._log.info('ready.')
# ..........................................................................
def set_tuning(self, p, i, d):
'''
Set the tuning of the power PID controller.
'''
self._kp = p
self._ki = i
self._kd = d
self._log.info(
'set PID tuning for {} motor: P={:>8.5f}; I={:>8.5f}; D={:>8.5f}.'.
format(self._orientation.name, self._kp, self._ki, self._kd))
# ..........................................................................
def get_tuning(self):
return [self._kp, self._ki, self._kd]
# ..........................................................................
def get_tuning_info(self):
'''
Returns the tuning information, formatted and including if the
setting is enabled or disabled.
'''
_en_p = 'on' if self._enable_p else 'off'
_en_i = 'on' if self._enable_i else 'off'
_en_d = 'on' if self._enable_d else 'off'
return [ '{:d}'.format(self._elapsed_sec),\
'{:6.4f} ({})'.format(self._kp, _en_p),\
'{:6.4f} ({})'.format(self._ki, _en_i),\
'{:6.4f} ({})'.format(self._kd, _en_d) ]
# ..............................................................................
def get_steps(self):
'''
A convenience method that returns the number of steps registered by the motor encoder.
'''
return self._motor.get_steps()
# ..............................................................................
def reset_steps(self):
'''
A convenience method that resets the step count to zero.
'''
self._motor.reset_steps()
# ..........................................................................
def is_stepping(self, direction):
'''
Returns True if the motor step count is less than the currently-set
step limit, or if traveling in REVERSE, greater than the step limit.
'''
# while f_is_enabled() and (( direction is Direction.FORWARD and self.get_steps() < _step_limit ) or \
# ( direction is Direction.REVERSE and self.get_steps() > _step_limit )):
# return ( self.get_steps() < self._step_limit ) if direction is Direction.FORWARD else ( self.get_steps() > self._step_limit )
if direction is Direction.FORWARD:
self._log.debug(
Fore.MAGENTA +
'{} is stepping forward? {:>5.2f} < {:>5.2f}'.format(
self._orientation, self.get_steps(), self._step_limit))
return (self.get_steps() < self._step_limit)
elif direction is Direction.REVERSE:
self._log.debug(
Fore.MAGENTA +
'{} is stepping reverse? {:>5.2f} > {:>5.2f}'.format(
self._orientation, self.get_steps(), self._step_limit))
return (self.get_steps() > self._step_limit)
else:
raise Exception('no direction provided.')
# ..........................................................................
def set_step_limit(self, direction, steps):
'''
Sets the step limit (i.e., the goal) prior to a call to step_to().
'''
if direction is Direction.FORWARD:
self._step_limit = self.get_steps() + steps
elif direction is Direction.REVERSE:
self._step_limit = self.get_steps() - steps
else:
raise Exception('no direction provided.')
# ..........................................................................
def set_velocity(self, velocity):
'''
Sets the velocity of the motor to the specified Velocity.
'''
if type(velocity) is Velocity:
self._target_velocity = velocity.value
elif isinstance(velocity, float):
self._target_velocity = velocity
else:
raise Exception('expected Velocity argument, not {}'.format(
type(velocity)))
# ..........................................................................
def reset_max_diff_steps(self):
self._max_diff_steps = 0
# ..........................................................................
def get_max_diff_steps(self):
return self._max_diff_steps
# ..........................................................................
def _loop(self):
'''
The PID loop that continues until the enabled flag is false.
'''
while self._enabled:
if self._read_p_from_pot:
self._kp = self._pot.get_scaled_value()
self._log.info(
Fore.BLUE + 'P={:>10.7f};'.format(self._kp) + Fore.BLACK +
' I={:>10.7f};'.format(self._ki) + Fore.BLACK +
' D={:>10.7f};'.format(self._kd) + Fore.GREEN +
Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format(
self._motor.get_velocity(), self._target_velocity))
elif self._read_i_from_pot:
self._ki = self._pot.get_scaled_value()
self._log.info(
Fore.BLACK + 'P={:>10.7f};'.format(self._kp) + Fore.BLUE +
' I={:>10.7f};'.format(self._ki) + Fore.BLACK +
' D={:>10.7f};'.format(self._kd) + Fore.GREEN +
Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format(
self._motor.get_velocity(), self._target_velocity))
elif self._read_d_from_pot:
self._kd = self._pot.get_scaled_value()
self._log.info(
Fore.BLACK + 'P={:>10.7f};'.format(self._kp) + Fore.BLACK +
' I={:>10.7f};'.format(self._ki) + Fore.BLUE +
' D={:>10.7f};'.format(self._kd) + Fore.GREEN +
Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format(
self._motor.get_velocity(), self._target_velocity))
else:
self._log.info(Fore.BLACK +
'P={:>10.7f}; I={:>10.7f}; D={:>10.7f}'.format(
self._kp, self._ki, self._kd))
_this_steps = self.get_steps()
_diff_steps = _this_steps - self._last_steps
self._max_diff_steps = max(self._max_diff_steps, _diff_steps)
# time.sleep(0.04)
self._log.debug('PID loop; diff steps: {:d}'.format(_diff_steps))
# TODO figure out how many ticks per loop is required for a given velocity
# self._log.info(Fore.GREEN + Style.BRIGHT + 'current velocity: {};'.format(self._motor.get_velocity()) \
# + Fore.CYAN + Style.NORMAL + ' target velocity: {}; step limit: {}.'.format(self._target_velocity, self._step_limit))
_changed = self._compute(self._target_velocity)
self._last_steps = _this_steps
self._rate.wait() # 20Hz
self._log.info(Fore.MAGENTA + Style.BRIGHT + 'exited PID loop.')
# ..........................................................................
def step_to(self, target_velocity, direction, slew_rate, f_is_enabled):
'''
Performs the PID calculation during a loop, returning once the
number of steps have been reached.
Velocity is the target velocity of the robot, expressed as
either an enumerated value or a decimal from 0.0 to 100.0,
where 50.0 is considered half-speed (though this will not be
true in practice, until we tune these values).
'''
if type(target_velocity) is not Velocity:
raise Exception('expected Velocity argument, not {}'.format(
type(target_velocity)))
_target_velocity = target_velocity.value
self._start_time = time.time()
if self._enable_slew:
if not self._slewlimiter.is_enabled():
print(Fore.GREEN + 'slew enabled.' + Style.RESET_ALL)
self._slewlimiter.set_rate_limit(slew_rate)
self._slewlimiter.enable()
self._slewlimiter.start()
else:
print(Fore.GREEN +
'slew disabled; f_is_enabled? {}'.format(f_is_enabled()) +
Style.RESET_ALL)
if direction is Direction.FORWARD: # .......................................................
_is_accelerating = self._motor.get_velocity() < _target_velocity
self._log.info(Fore.GREEN + Style.BRIGHT + 'current velocity: {};'.format(self._motor.get_velocity()) \
+ Fore.CYAN + Style.NORMAL + ' target velocity: {}; step limit: {}; is_accelerating: {}.'.format(_target_velocity, self._step_limit, _is_accelerating))
while f_is_enabled() and self.is_stepping(
direction
): # or ( direction is Direction.REVERSE and self.get_steps() > self._step_limit )):
if self._enable_slew:
_slewed_target_velocity = self._slewlimiter.slew(
self._motor.get_velocity(), _target_velocity)
_changed = self._compute(_slewed_target_velocity)
else:
_changed = self._compute(_target_velocity)
self._log.debug(Fore.BLACK + Style.DIM +
'{:d}/{:d} steps.'.format(
self.get_steps(), self._step_limit))
if not _changed and self._motor.get_velocity(
) == 0.0 and _target_velocity == 0.0 and self._step_limit > 0: # we will never get there if we aren't moving
self._log.info(Fore.RED + 'break 2.')
break
# displays info only:
if _is_accelerating and self._motor.get_velocity(
) >= _target_velocity:
_elapsed = time.time() - self._start_time
_is_accelerating = False
self._log.info(
Fore.GREEN + Style.BRIGHT +
'reached target velocity of {:+06.2f} at {:5.2f} sec elapsed.'
.format(_target_velocity, _elapsed))
self._rate.wait() # 20Hz
if self._motor.is_interrupted():
self._log.info(Fore.RED + 'motor interrupted.')
break
if self._orientation is Orientation.PORT:
self._log.info(Fore.RED + 'current velocity: {:>5.2f};'.format(self._motor.get_velocity() ) \
+ Fore.CYAN + ' target velocity: {}; {:d} steps of: {}; is_accelerating: {}.'.format(_target_velocity, self.get_steps(), self._step_limit, _is_accelerating))
else:
self._log.info(Fore.GREEN + 'current velocity: {:>5.2f};'.format(self._motor.get_velocity() ) \
+ Fore.CYAN + ' target velocity: {}; {:d} steps of: {}; is_accelerating: {}.'.format(_target_velocity, self.get_steps(), self._step_limit, _is_accelerating))
elif direction is Direction.REVERSE: # .....................................................
_target_velocity = -1.0 * _target_velocity
_is_accelerating = self._motor.get_velocity() < _target_velocity
self._log.info(
Fore.RED + Style.BRIGHT +
'target velocity: {}; step limit: {}; is_accelerating: {}.'.
format(_target_velocity, self._step_limit, _is_accelerating))
while f_is_enabled() and self.is_stepping(
direction
): # or ( direction is Direction.REVERSE and self.get_steps() > self._step_limit )):
if self._enable_slew:
_slewed_target_velocity = self._slewlimiter.slew(
self._motor.get_velocity(), _target_velocity)
_changed = self._compute(_slewed_target_velocity)
else:
_changed = self._compute(_target_velocity)
self._log.debug(Fore.BLACK + Style.DIM +
'{:d}/{:d} steps.'.format(
self.get_steps(), self._step_limit))
if not _changed and self._motor.get_velocity(
) == 0.0 and _target_velocity == 0.0 and self._step_limit > 0: # we will never get there if we aren't moving
break
# displays info only:
if _is_accelerating and self._motor.get_velocity(
) >= _target_velocity:
_elapsed = time.time() - self._start_time
_is_accelerating = False
self._log.info(
Fore.GREEN + Style.BRIGHT +
'reached target velocity of {:+06.2f} at {:5.2f} sec elapsed.'
.format(_target_velocity, _elapsed))
self._rate.wait() # 20Hz
if self._motor.is_interrupted():
break
else: # ....................................................................................
raise Exception('no direction provided.')
_elapsed = time.time() - self._start_time
self._elapsed_sec = int(round(_elapsed)) + 1
self._log.info(
'{} motor step-to complete; {:5.2f} sec elapsed;'.format(
self._orientation.name, _elapsed) + Fore.MAGENTA +
' {:d}/{:d} steps.'.format(self.get_steps(), self._step_limit))
if self._enable_slew: # if not a repeat call
self._slewlimiter.reset(0.0)
self._motor.reset_interrupt()
# ..........................................................................
def _compute(self, target_velocity):
'''
The PID power compute function, called upon each loop.
Returns True if PID computed a change, False if the error was zero and no change was applied.
'''
self._loop_count = next(self._counter)
# compare current and target velocities ............................
_current_velocity = self._motor.get_velocity()
_error = target_velocity - _current_velocity
# if _current_velocity == 0:
# self._log.debug(Fore.CYAN + Style.NORMAL + '[{:0d}]; velocity: {:+06.2f} ➔ {:+06.2f}\t _error: {:>5.2f}.'.format(self._loop_count, _current_velocity, target_velocity, _error))
# else:
# self._log.debug(Fore.CYAN + Style.BRIGHT + '[{:0d}]; velocity: {:+06.2f} ➔ {:+06.2f}\t _error: {:>5.2f}.'.format(self._loop_count, _current_velocity, target_velocity, _error))
_current_power = self._motor.get_current_power_level() * (
1.0 / self._motor.get_max_power_ratio())
_power_level = _current_power
if PID.equals_zero(_error):
if target_velocity == 0.0:
self._motor.set_motor_power(0.0)
self._log.info(
Fore.WHITE + Style.BRIGHT +
'[{:02d}]+ NO ERROR, currently at zero velocity.'.format(
self._loop_count))
else:
self._log.info(Fore.WHITE + Style.BRIGHT + '[{:02d}]+ no error, currently at target velocity: {:+06.2f}; at power: {:+5.3f}'.format(\
self._loop_count, _current_velocity, _current_power))
# remember some variables for next time ........................
self._last_error = 0.0
_changed = False
else:
# P: Proportional ..............................................
if self._enable_p:
_p_diff = self._kp * _error
self._log.debug(Fore.BLUE + Style.BRIGHT +
'P diff: {:>5.4f}'.format(_p_diff) +
Style.NORMAL +
' = self._kp: {:>5.4f} * _error: {:>5.4f}.'.
format(self._kp, _error))
else:
_p_diff = 0.0
# I: Integral ..................................................
if self._enable_i:
_i_diff = self._ki * self._sum_errors if self._enable_i else 0.0
self._log.debug(Fore.MAGENTA + Style.BRIGHT + 'I diff: {:45.4f}'.format(_i_diff) + Style.NORMAL \
+ ' = self._ki: {:>5.4f} * _sum_errors: {:>5.4f}.'.format(self._ki, self._sum_errors))
else:
_i_diff = 0.0
# D: Derivative ................................................
if self._enable_d:
_d_diff = self._kd * self._last_error if self._enable_d else 0.0
self._log.debug(
Fore.YELLOW + 'D diff: {:>5.4f}'.format(_d_diff) +
Style.NORMAL +
' = self._kd: {:>5.4f} * _last_error: {:>5.4f}.'.format(
self._kd, self._last_error))
else:
_d_diff = 0.0
# calculate output .............................................
_output = _p_diff + _i_diff + _d_diff
# self._log.debug(Fore.CYAN + Style.BRIGHT + '_output: {:>5.4f}'.format(_output) + Style.NORMAL + ' = (P={:+5.4f}) + (I={:+5.4f}) + (D={:+5.4f})'.format(_p_diff, _i_diff, _d_diff))
# clipping .....................................................
_clipped_output = self._clip(_output)
# set motor power ..............................................
_power_level = _current_power + _clipped_output
# if abs(_output) <= 0.0005: # we're pretty close to the right value
# self._log.debug(Fore.WHITE + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \
# + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\
# _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff))
# elif _output >= 0: # we're going too slow
# self._log.debug(Fore.GREEN + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \
# + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\
# _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff))
# else: # we're going too fast
# self._log.debug(Fore.RED + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \
# + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\
# _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff))
self._motor.set_motor_power(_power_level)
# remember some variables for next time ........................
self._last_error = _error
self._sum_errors += _error
_changed = True
# if configured, capture odometry data ...................
if self._filewriter:
_elapsed = time.time() - self._start_time
_data = '{:>5.3f}\t{:>5.2f}\t{:>5.2f}\t{:>5.2f}\t{:d}\n'.format(
_elapsed, (_power_level * 100.0), _current_velocity,
target_velocity, self.get_steps())
self._stats_queue.append(_data)
self._log.debug('odometry:' + Fore.BLACK + ' time: {:>5.3f};\tpower: {:>5.2f};\tvelocity: {:>5.2f}/{:>5.2f}\tsteps: {:d}'.format(\
_elapsed, _power_level, _current_velocity, target_velocity, self.get_steps()))
return _changed
# ..........................................................................
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 for {} motor enabled.'.format(
self._orientation))
else:
self._log.warning(
'cannot enable PID loop for {} motor: thread already exists.'
.format(self._orientation))
else:
self._log.warning(
'cannot enable PID loop for {} motor: already closed.'.format(
self._orientation))
# ..........................................................................
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 for {} motor disabled.'.format(
self._orientation))
# ..........................................................................
def close(self):
self.close_filewriter()
self._log.info('closed.')
# filewriter support .......................................................
# ..............................................................................
def set_filewriter(self, filewriter):
'''
If a FileWriter is set then the Motor will generate odometry data
and write this to a data file whose directory and filename are
determined by configuration.
'''
self._filewriter = filewriter
if self._filewriter:
if self._stats_queue is None:
self._stats_queue = deque()
self._filewriter.enable(self._stats_queue)
self._log.info('filewriter enabled.')
# ..........................................................................
def close_filewriter(self):
if not self._filewriter_closed:
self._filewriter_closed = True
if self._filewriter:
_tuning = self.get_tuning_info()
self._filewriter.write_gnuplot_settings(_tuning)
self._filewriter.disable()
# formulae .................................................................
# ..........................................................................
@staticmethod
def equals_zero(value):
'''
Returns True if the value is within 2% of 0.0.
'''
return numpy.isclose(value, 0.0, rtol=0.02, atol=0.0)
# ..............................................................................
def get_distance_cm_for_steps(self, steps):
'''
Provided a number of steps returns the distance traversed.
'''
return steps / self._steps_per_cm
# ..............................................................................
def get_steps_for_distance_cm(self, distance_cm):
'''
A function that returns the number of forward steps require to
traverse the distance (measured in cm). Returns an int.
'''
'''
Geometry Notes ...................................
494 encoder steps per rotation (maybe 493)
68.5mm diameter tires
215.19mm/21.2cm wheel circumference
1 wheel rotation = 215.2mm
2295 steps per meter
2295 steps per second = 1 m/sec
2295 steps per second = 100 cm/sec
229.5 steps per second = 10 cm/sec
22.95 steps per second = 1 cm/sec
1 rotation = 215mm = 494 steps
1 meter = 4.587 rotations
2295.6 steps per meter
22.95 steps per cm
'''
return round(distance_cm * self._steps_per_cm)
# ..............................................................................
def get_forward_steps(self, rotations):
'''
A function that return the number of steps corresponding to the number
of wheel rotations.
'''
return self._step_per_rotation * rotations
def main():
_log = Logger("vel-test", Level.INFO)
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_motors = Motors(_config, None, Level.INFO)
_port_pid = _motors.get_motor(Orientation.PORT).get_pid_controller()
_stbd_pid = _motors.get_motor(Orientation.STBD).get_pid_controller()
_pid = _stbd_pid
Velocity.CONFIG.configure(_config)
# sys.exit(0)
try:
_trial_time_sec = 10.0
# _motor_power = 0.99
_motor_velocity = 0.0
_target_velocity = 80.0
# _motors.get_motor(Orientation.PORT).get_thunderborg().SetMotor1(_motor_power) # direct to TB (max: 0.6?)
# _motors.get_motor(Orientation.STBD).get_thunderborg().SetMotor2(_motor_power) # direct to TB (max: 0.6?)
# _distance_cm = 100
# _fwd_steps_per_meter = _stbd_pid.get_steps_for_distance_cm(_distance_cm)
# _log.info(Fore.MAGENTA + Style.NORMAL + 'calculated: {:>5.2f} steps per {:d} cm.'.format(_fwd_steps_per_meter, _distance_cm))
_current_value = 0
_limiter = SlewLimiter(_config, Orientation.STBD, Level.INFO)
_limiter.enable()
_rate = Rate(5) # Hz
_pid.reset_steps()
_pid.enable()
# accelerate...
# _motors.get_motor(Orientation.PORT).accelerate_to_velocity(_motor_velocity, SlewRate.NORMAL, -1)
while _motor_velocity < _target_velocity:
_motor_velocity = _limiter.slew(_motor_velocity, _target_velocity)
_pid.set_velocity(_motor_velocity)
_log.info(Fore.CYAN + Style.BRIGHT +
'velocity: {:>5.2f} -> {:>5.2f}.'.format(
_motor_velocity, _target_velocity))
_rate.wait()
# time.sleep(0.1)
_log.info(Fore.GREEN + Style.BRIGHT +
'reached terminal velocity: {:>5.2f} == {:>5.2f}.'.format(
_motor_velocity, _target_velocity))
# _motors.get_motor(Orientation.PORT).set_motor_power(_motor_power)
time.sleep(_trial_time_sec)
# decelerate...
_target_velocity = 0.0
while _motor_velocity > _target_velocity:
_motor_velocity = _limiter.slew(_motor_velocity, _target_velocity)
_pid.set_velocity(_motor_velocity)
_log.info(Fore.YELLOW + Style.NORMAL +
'velocity: {:>5.2f} -> {:>5.2f}.'.format(
_motor_velocity, _target_velocity))
_rate.wait()
_pid.disable()
time.sleep(1.0)
_motors.brake()
# time.sleep(1.0)
# _motors.get_motor(Orientation.STBD).accelerate_to_velocity(_motor_velocity, SlewRate.NORMAL, -1)
# _stbd_pid.reset_steps()
# _motors.get_motor(Orientation.STBD).set_motor_power(_motor_power)
# _stbd_pid.enable()
# time.sleep(_trial_time_sec)
# _stbd_pid.disable()
# _motors.brake()
# output results of test:
_log.info(
Fore.MAGENTA + Style.BRIGHT +
'max diff steps: {:d} steps per loop @ motor velocity of {:5.2f}'.
format(_pid.get_max_diff_steps(), _motor_velocity))
# _log.info(Fore.MAGENTA + Style.BRIGHT + 'STBD max diff steps: {:d} steps per loop @ motor velocity of {:5.2f}'.format(_stbd_pid.get_max_diff_steps(), _motor_velocity))
sys.exit(0)
# _forward_steps = _fwd_steps_per_meter
# _slew_rate = SlewRate.FAST
# _direction = Direction.FORWARD
# _velocity = Velocity.HALF
# _log.info(Fore.RED + 'FORWARD... ---------------------------------- ')
# go(_motors, _velocity, _slew_rate, Direction.FORWARD, _forward_steps)
# _log.info(Fore.RED + 'BRAKING... ---------------------------------- ')
# maintain velocity for 1 m
# _distance_m = 2
# _port_step_limit = _port_pid.get_steps() + _fwd_steps_per_meter * _distance_m
# _stbd_step_limit = _stbd_motor.get_steps() + _fwd_steps_per_meter * _distance_m
# _port_pid.maintain_velocity(_velocity, _port_step_limit)
# _stbd_motor.maintain_velocity(_velocity, _stbd_step_limit)
# time.sleep(1.0)
# _log.info(Fore.RED + 'REVERSE... ---------------------------------- ')
# go(_motors, _velocity, _slew_rate, Direction.REVERSE, _forward_steps)
# _port_pid.maintain_velocity(_velocity, _port_step_limit)
# _stbd_motor.maintain_velocity(_velocity, _stbd_step_limit)
# _motors.brake()
_log.info(Fore.CYAN + Style.BRIGHT + 'completed thread.')
# _log.info(Fore.CYAN + Style.BRIGHT + 'A. motor test complete; intended: {:d}; actual steps: port: {} ; stbd: {}.'.format(_forward_steps, _pid.get_steps(), _stbd_pid.get_steps()))
_distance_cm = _pid.get_distance_cm_for_steps(_pid.get_steps())
_log.info(Fore.CYAN + Style.BRIGHT +
'distance traveled: {:>5.2f}.'.format(_distance_cm))
# _port_distance_cm = _port.get_distance_cm_for_steps(_pid.get_steps())
# _stbd_distance_cm = _stbd_pid.get_distance_cm_for_steps(_stbd_pid.get_steps())
# _log.info(Fore.CYAN + Style.BRIGHT + 'distance traveled: port: {:>5.2f}, stbd: {:>5.2f}.'.format(_port_distance_cm, _stbd_distance_cm))
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'B. motor test complete; intended: {:d}; actual steps: port: {} ; stbd: {}.'.format(\
_forward_steps, _port_pid.get_steps(), _stbd_pid.get_steps()))
_stbd_motor.halt()
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.')
# _stbd_motor.halt()
quit()