def main():
try:
_log = Logger("test", Level.INFO)
_log.info(Fore.BLUE + 'configuring...')
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_message_bus = MessageBus(Level.INFO)
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_clock.enable()
_ifs = IntegratedFrontSensor(_config, _clock, Level.INFO)
_ifs.enable()
_handler = MessageHandler(Level.INFO)
_message_bus.add_handler(Message, _handler.handle)
try:
_log.info(Fore.BLUE + 'starting loop... type Ctrl-C to exit.')
_rate = Rate(1)
# while True:
for i in range(360):
_rate.wait()
_log.info(Fore.BLUE + 'exited loop.')
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
except Exception as e:
print(Fore.RED + Style.BRIGHT + 'error: {}'.format(e))
traceback.print_exc(file=sys.stdout)
def __init__(self, config, queue, message_factory, level):
'''
Parameters:
config: the YAML-based application configuration
queue: the message queue to receive messages from this task
message_factory: the factory for creating messages
mutex: vs godzilla
'''
if config is None:
raise ValueError('no configuration provided.')
self._level = level
self._log = Logger("gamepad", level)
self._log.info('initialising...')
self._config = config
_config = config['ros'].get('gamepad')
# config
_loop_freq_hz = _config.get('loop_freq_hz')
self._rate = Rate(_loop_freq_hz)
self._device_path = _config.get('device_path')
self._queue = queue
self._message_factory = message_factory
self._gamepad_closed = False
self._closed = False
self._enabled = False
self._thread = None
self._gamepad = None
def main():
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_message_factory = MessageFactory(Level.INFO)
_queue = MessageQueue(_message_factory, Level.INFO)
_rate = Rate(5) # 20Hz
_nxp9dof = NXP9DoF(_config, _queue, Level.INFO)
_nxp = NXP(_nxp9dof, Level.INFO)
# _nxp.enable()
# _nxp9dof.enable()
_nxp.ahrs2(True)
while True:
# _nxp.heading(20)
# _nxp.ahrs(10)
# _nxp.imu(10)
_nxp.ahrs2(False)
_rate.wait()
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught; exiting...' + Style.RESET_ALL)
def test_rot_control():
_log = Logger("rot-ctrl-test", Level.INFO)
_rot_ctrl = None
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
# def __init__(self, config, minimum, maximum, step, level):
# _min = -127
# _max = 127
# _step = 1
_min = 0
_max = 10
_step = 0.5
_rot_ctrl = RotaryControl(_config, _min, _max, _step, Level.INFO)
_rate = Rate(20)
_log.info(Fore.WHITE + Style.BRIGHT +
'waiting for changes to rotary encoder...')
while True:
_value = _rot_ctrl.read()
_log.info(Fore.YELLOW + ' value: {:5.2f}'.format(_value))
_rate.wait()
finally:
if _rot_ctrl:
_log.info('resetting rotary encoder...')
_rot_ctrl.reset()
def main(argv):
try:
# read YAML configuration
_level = Level.INFO
_loader = ConfigLoader(_level)
filename = 'config.yaml'
_config = _loader.configure(filename)
_motors = Motors(_config, None, Level.INFO)
_blob = Blob(_config, _motors, _level)
_blob.enable()
_rate = Rate(1)
# _blob.capture()
# for i in range(32):
while True:
print(Fore.BLACK + Style.DIM + 'capturing...' + Style.RESET_ALL)
# _location = _blob.capture()
# print(Fore.YELLOW + Style.NORMAL + 'captured location: {}'.format(_location) + Style.RESET_ALL)
# print('captured.')
_rate.wait()
_blob.disable()
except KeyboardInterrupt:
print(Fore.CYAN + 'Ctrl-C interrupt.' + Style.RESET_ALL)
except Exception:
print(Fore.RED + Style.BRIGHT + 'error starting ros: {}'.format(traceback.format_exc()) + Style.RESET_ALL)
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 _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...')
_heading = None
_pitch = None
_roll = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
# grab data at 10Hz
rate = Rate(10)
while not self._closed:
if self._enabled:
header = 91
print(Fore.CYAN + ('-'*header) + Style.RESET_ALL)
print(Fore.CYAN + "| {:17} | {:20} | {:20} | {:21} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading") + Style.RESET_ALL)
print(Fore.CYAN + ('-'*header) + Style.RESET_ALL)
for _ in range(10):
a, m, g = self._imu.get()
r, p, h = self._imu.getOrientation(a, m)
deg = Convert.to_degrees(h)
# self._log.info(Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(a[0], a[1], a[2], r, p, h) + Style.RESET_ALL)
print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} '.format(a[0], a[1], a[2])
+ Fore.YELLOW + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(m[0], m[1], m[2])
+ Fore.MAGENTA + '| {:>6.1f} {:>6.1f} {:>6.1f} '.format(g[0], g[1], g[2])
+ Fore.GREEN + '| {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
time.sleep(0.50)
print('-'*header)
print(' uT: micro Tesla')
print(' g: gravity')
print('dps: degrees per second')
print('')
# accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2
# mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
# gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
#
# print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\
# + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\
# + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL)
rate.wait()
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
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 __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.')
def _monitor(self, f_is_enabled):
'''
A 20Hz loop that prints PID statistics to the log while enabled. Note that this loop
is not synchronised with the two PID controllers, which each have their own loop.
'''
_indent = ' '
self._log.info('PID monitor header:\n' \
+ _indent + 'name : description\n' \
+ _indent + 'kp : proportional constant\n' \
+ _indent + 'ki : integral constant\n' \
+ _indent + 'kd : derivative constant\n' \
+ _indent + 'p_cp : port proportial value\n' \
+ _indent + 'p_ci : port integral value\n' \
+ _indent + 'p_cd : port derivative value\n' \
+ _indent + 'p_lpw : port last power\n' \
+ _indent + 'p_cpw : port current motor power\n' \
+ _indent + 'p_spwr : port set power\n' \
+ _indent + 'p_cvel : port current velocity\n' \
+ _indent + 'p_stpt : port velocity setpoint\n' \
+ _indent + 's_cp : starboard proportial value\n' \
+ _indent + 's_ci : starboard integral value\n' \
+ _indent + 's_cd : starboard derivative value\n' \
+ _indent + 's_lpw : starboard proportional value\n' \
+ _indent + 's_cpw : starboard integral value\n' \
+ _indent + 's_spw : starboard derivative value\n' \
+ _indent + 's_cvel : starboard current velocity\n' \
+ _indent + 's_stpt : starboard velocity setpoint\n' \
+ _indent + 'p_stps : port encoder steps\n' \
+ _indent + 's_stps : starboard encoder steps')
if self._log_to_file:
self._file_log = Logger("pid", Level.INFO, log_to_file=True)
# write header
self._file_log.file(
"kp|ki|kd|p_cp|p_ci|p_cd|p_lpw|p_cpw|p_spwr|p_cvel|p_stpt|s_cp|s_ci|s_cd|s_lpw|s_cpw|s_spw|s_cvel|s_stpt|p_stps|s_stps|"
)
self._log.info(Fore.GREEN + 'starting PID monitor...')
_rate = Rate(20)
while f_is_enabled():
kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps = self._port_pid.stats
_x, _y, _z, s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps = self._stbd_pid.stats
_msg = ('{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:>5.2f}|{:d}|').format(\
kp, ki, kd, p_cp, p_ci, p_cd, p_last_power, p_current_motor_power, p_power, p_current_velocity, p_setpoint, p_steps, \
s_cp, s_ci, s_cd, s_last_power, s_current_motor_power, s_power, s_current_velocity, s_setpoint, s_steps)
_p_hilite = Style.BRIGHT if p_power > 0.0 else Style.NORMAL
_s_hilite = Style.BRIGHT if s_power > 0.0 else Style.NORMAL
_msg2 = (Fore.RED+_p_hilite+'{:7.4f}|{:7.4f}|{:7.4f}|{:<5.2f}|{:<5.2f}|{:<5.2f}|{:>5.2f}|{:d}'+Fore.GREEN+_s_hilite+'|{:7.4f}|{:7.4f}|{:7.4f}|{:5.2f}|{:5.2f}|{:<5.2f}|{:<5.2f}|{:d}|').format(\
p_cp, p_ci, p_cd, p_last_power, p_power, p_current_velocity, p_setpoint, p_steps, \
s_cp, s_ci, s_cd, s_last_power, s_power, s_current_velocity, s_setpoint, s_steps)
_rate.wait()
if self._log_to_file:
self._file_log.file(_msg)
if self._log_to_console:
self._log.info(_msg2)
self._log.info('PID monitor stopped.')
def __init__(self, loop_freq_hz, callback, level=Level.INFO):
super().__init__()
self._log = Logger("clock", level)
self._loop_freq_hz = loop_freq_hz
self._rate = Rate(self._loop_freq_hz)
self._log.info('tick frequency: {:d}Hz'.format(self._loop_freq_hz))
self._callbacks = []
self._thread = None
self._enabled = False
self._closed = False
self._log.info('ready.')
def capture(self):
if not self._enabled:
raise Exception('not enabled.')
self._log.info(Fore.GREEN + Style.BRIGHT + 'snapshot start...')
_rate = Rate(3) # 10Hz
self._capturing = True
while self._capturing == True: # wait til we're done
self._log.info(Fore.BLACK + '...')
_rate.wait()
_location = self._location
self._log.info(Fore.GREEN + Style.BRIGHT +
'snapshot complete; location: '.format(self._location))
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.INFO)
# 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._rate = Rate(
_config.get('sample_freq_hz')) # 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._log.info('ready.')
def __init__(self, config, clock, level):
if config is None:
raise ValueError('no configuration provided.')
if clock is None:
raise ValueError('no clock provided.')
self._clock = clock
self._message_bus = clock.message_bus
self._message_factory = clock.message_factory
self._log = Logger("ifs", level)
self._log.info('configuring integrated front sensor...')
self._config = config['ros'].get('integrated_front_sensor')
self._ignore_duplicates = self._config.get('ignore_duplicates')
_use_pot = self._config.get('use_potentiometer')
self._pot = Potentiometer(config, Level.INFO) if _use_pot else None
self._loop_freq_hz = self._config.get('loop_freq_hz')
self._rate = Rate(self._loop_freq_hz)
# event thresholds:
self._cntr_raw_min_trigger = self._config.get('cntr_raw_min_trigger')
self._oblq_raw_min_trigger = self._config.get('oblq_raw_min_trigger')
self._side_raw_min_trigger = self._config.get('side_raw_min_trigger')
self._cntr_trigger_distance_cm = self._config.get(
'cntr_trigger_distance_cm')
self._oblq_trigger_distance_cm = self._config.get(
'oblq_trigger_distance_cm')
self._side_trigger_distance_cm = self._config.get(
'side_trigger_distance_cm')
self._log.info('event thresholds: \t' \
+ Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._side_trigger_distance_cm, self._oblq_trigger_distance_cm) \
+ Fore.BLUE + ' center={:>5.2f};'.format(self._cntr_trigger_distance_cm) \
+ Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._oblq_trigger_distance_cm, self._side_trigger_distance_cm))
# hardware pin assignments are defined in IO Expander
# create/configure IO Expander
self._ioe = IoExpander(config, Level.INFO)
# these are used to support running averages
_queue_limit = 2 # larger number means it takes longer to change
self._deque_cntr = Deque([], maxlen=_queue_limit)
self._deque_port = Deque([], maxlen=_queue_limit)
self._deque_stbd = Deque([], maxlen=_queue_limit)
self._deque_port_side = Deque([], maxlen=_queue_limit)
self._deque_stbd_side = Deque([], maxlen=_queue_limit)
self._counter = itertools.count()
self._thread = None
self._group = 0
self._enabled = False
self._suppressed = False
self._closed = False
self._count = 0
self._log.info('ready.')
def _cruise(self, pid, cruising_velocity, f_is_enabled):
'''
Threaded method for roaming.
'''
_rate = Rate(20)
if not pid.enabled:
pid.enable()
pid.set_slew_rate(SlewRate.NORMAL)
pid.enable_slew(True)
self._cruising_velocity = cruising_velocity
self._log.info(Fore.YELLOW +
'{} motor accelerating to velocity: {:5.2f}...'.format(
pid.orientation.label, self._cruising_velocity))
pid.velocity = self._cruising_velocity
while pid.velocity < self._cruising_velocity:
pid.velocity += 5.0
self._log.info(Fore.GREEN + 'accelerating from {} to {}...'.format(
pid.velocity, self._cruising_velocity))
time.sleep(0.5)
# _rate.wait()
self._log.info(Fore.YELLOW +
'{} motor cruising...'.format(pid.orientation.label))
while f_is_enabled:
self._log.info(Fore.BLACK +
'{} motor cruising at velocity: {:5.2f}...'.format(
pid.orientation.label, pid.velocity))
time.sleep(0.5)
self._log.info(
Fore.YELLOW +
'{} motor decelerating...'.format(pid.orientation.label))
pid.velocity = 0.0
while pid.velocity > 0.0:
pid.velocity -= 5.0
self._log.info(Fore.GREEN + 'decelerating from {} to {}...'.format(
pid.velocity, self._cruising_velocity))
time.sleep(0.01)
# _rate.wait()
self._log.info(Fore.YELLOW +
'{} motor at rest.'.format(pid.orientation.label))
time.sleep(1.0)
pid.set_slew_rate(SlewRate.NORMAL)
self._log.info(Fore.YELLOW + 'cruise complete.')
def test_selector():
_log = Logger("sel-test", Level.INFO)
# _i2c_scanner = I2CScanner(Level.WARN)
# if not _i2c_scanner.has_address([0x0F]):
# _log.warning('test ignored: no rotary encoder found.')
# return
_selector = None
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_selector = Selector(_config, Level.INFO)
_count = 0
_updates = 0
_rate = Rate(20)
_last_value = 0
_limit = 45
_log.info('live-updating from rotary encoder...')
while _updates < _limit:
_value = _selector.read()
if _value != _last_value:
_updates += 1
_log.info(Style.BRIGHT +
'returned value: {:d}; updates remaining: {:d}'.
format(_value, _limit - _updates))
else:
_log.info(Style.DIM +
'returned value: {:d}; updates remaining: {:d}'.
format(_value, _limit - _updates))
_last_value = _value
_count += 1
if _count % 33 == 0:
_log.info('waiting…')
_rate.wait()
finally:
if _selector:
_selector.reset()
def _loop(self, _enabled):
self._log.info(Fore.RED + 'start _loop...') # TEMP
_rate = Rate(1) # 1Hz
_thread_count = 4
try:
# summarise elements on a column basis .........
self._log.info('starting image capture...')
with picamera.PiCamera() as camera:
camera.resolution = (self._width, self._height)
camera.iso = 800
camera.exposure_mode = 'auto'
camera.framerate = 15
# https://picamera.readthedocs.io/en/release-1.13/_modules/picamera/array.html#PiRGBArray
camera.start_preview()
# time.sleep(2)
self._log.info(Style.BRIGHT + '1. creating processor pool...')
self._pool = [ImageProcessor(self._config, camera, i, _enabled) for i in range(_thread_count)]
self._log.info(Style.BRIGHT + '2. calling camera.capture_continuous...')
# size = ( self._width, self._height )
# camera.capture_continuous(self.outputs, format='rgb', resize=size, use_video_port=True)
# camera.capture_sequence(self.outputs, format='rgb', resize=size, use_video_port=True)
# camera.capture_sequence(self.outputs, format='rgb', use_video_port=True)
camera.capture_sequence(self.outputs, format='rgb', use_video_port=True)
self._log.info(Style.BRIGHT + '3. called camera.capture_continuous...')
time.sleep(3)
_rate.wait()
# self._close_pool()
self._log.info('end _loop.') # TEMP
except picamera.PiCameraMMALError:
self._log.error('could not get camera: in use by another process.')
except Exception as e:
self._log.error('error in blob loop: {}'.format(e))
self._log.error('traceback in blob loop: {}'.format(traceback.format_exc()))
finally:
self._log.set_suppress(False)
self._motors.set_led_show_battery(True)
self._log.info('finis.')
def test_pot():
'''
Tests reading the wiper of a potentiometer attached to a GPIO pin,
where its other two connections are to Vcc and ground.
'''
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_pot = Potentiometer(_config, Level.DEBUG)
_value = 0
# start...
_rate = Rate(20) # Hz
# i = 0
# while True:
for i in range(20):
_value = _pot.get_value()
_scaled_value = _pot.get_scaled_value()
_log.info('[{:d}] analog value: {:03d};'.format(i, _value) +
Fore.GREEN + '\tscaled: {:>7.4f};'.format(_scaled_value))
_rate.wait()
def test_rot_encoder():
_log = Logger("rot-test", Level.INFO)
_i2c_scanner = I2CScanner(Level.WARN)
if not _i2c_scanner.has_address([0x16]):
_log.warning('test ignored: no rotary encoder found.')
return
_rot = None
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_rot = RotaryEncoder(_config, 0x0F, Level.INFO)
_count = 0
_updates = 0
_update_led = True
_last_value = 0
_rate = Rate(20)
_log.info(Fore.WHITE + Style.BRIGHT +
'waiting for rotary encoder to make 10 ticks...')
while _updates < 10:
# _value = _rot.update() # original method
_value = _rot.read(_update_led) # improved method
if _value != _last_value:
_log.info(Style.BRIGHT + 'returned value: {:d}'.format(_value))
_updates += 1
_last_value = _value
_count += 1
if _count % 33 == 0:
_log.info(Fore.BLACK + Style.BRIGHT + 'waiting…')
_rate.wait()
finally:
if _rot:
_log.info('resetting rotary encoder...')
_rot.reset()
def test_ioe_potentiometer():
_log = Logger("test-ioe-pot", Level.INFO)
_log.info(Fore.RED + 'to kill type Ctrl-C')
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
_config = _loader.configure('config.yaml')
_pot = Potentiometer(_config, Level.INFO)
_pot.set_output_limits(0.00, 0.150)
_log.info('starting test...')
_hz = 10
_rate = Rate(_hz, Level.ERROR)
while True:
# _value = self.get_value()
# self.set_rgb(_value)
# _scaled_value = self.scale_value() # as float
_scaled_value = _pot.get_scaled_value(True)
# _scaled_value = math.floor(self.scale_value()) # as integer
_log.info(Fore.YELLOW + 'scaled value: {:9.6f}'.format(_scaled_value))
_rate.wait()
def main(argv):
try:
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_pot = Potentiometer(_config, Level.INFO)
# start...
_rate = Rate(20) # Hz
while True:
_value = _pot.get_value()
_scaled_value = _pot.get_scaled_value()
_log.info(' analog value: {:03d};'.format(_value) + Fore.GREEN + '\tscaled: {:>7.4f};'.format(_scaled_value))
_rate.wait()
except KeyboardInterrupt:
_log.info('caught Ctrl-C; exiting...')
except Exception:
_log.error('error starting potentiometer: {}'.format(traceback.format_exc()))
def _handbuilt_imu(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...')
_heading = None
_pitch = None
_roll = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
# grab data at 20Hz
_rate = Rate(1)
header = 90
print('-' * header)
# print("| {:17} | {:20} |".format("Accels [g's]", " IMU Accels"))
print("| {:17} |".format("Mag [xyz]"))
while not self._closed:
if self._enabled:
accel_x, accel_y, accel_z = self._fxos.accelerometer # m/s^2
gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
a, m, g = self._imu.get()
mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
mag_x_g = m[0] * 100.0
mag_y_g = m[1] * 100.0
mag_z_g = m[2] * 100.0
# rate_gyro_x_dps = Convert.rps_to_dps(gyro_x)
# rate_gyro_y_dps = Convert.rps_to_dps(gyro_y)
# rate_gyro_z_dps = Convert.rps_to_dps(gyro_z)
# mag_x_g = mag_x * 100.0
# mag_y_g = mag_y * 100.0
# mag_z_g = mag_z * 100.0
heading = 180.0 * math.atan2(mag_y_g, mag_x_g) / math.pi
print(Fore.CYAN + '| x{:>6.3f} y{:>6.3f} z{:>6.3f} |'.format( mag_x_g, mag_y_g, mag_z_g) \
+ Style.BRIGHT + '\t{:>5.2f}'.format(heading) + Style.RESET_ALL)
# a, m, g = self._imu.get()
# print(Fore.CYAN + '| {:>6.3f} {:>6.3f} {:>6.3f} | {:>6.3f} {:>6.3f} {:>6.3f} |'.format( accel_x, accel_y, accel_z, a[0], a[1], a[2]) + Style.RESET_ALL)
_rate.wait()
# + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
# time.sleep(0.50)
# mag_x, mag_y, mag_z = self._fxos.magnetometer # uTesla
# gyro_x, gyro_y, gyro_z = self._fxas.gyroscope # radians/s
# header = 90
# print('-'*header)
# print("| {:17} | {:20} | {:20} | {:20} |".format("Accels [g's]", " Magnet [uT]", "Gyros [dps]", "Roll, Pitch, Heading"))
# print('-'*header)
# for _ in range(10):
# a, m, g = self._imu.get()
# r, p, h = self._imu.getOrientation(a, m)
# deg = Convert.to_degrees(h)
# print(Fore.CYAN + '| {:>5.2f} {:>5.2f} {:>5.2f} | {:>6.1f} {:>6.1f} {:>6.1f} | {:>6.1f} {:>6.1f} {:>6.1f} |'.format(
# a[0], a[1], a[2],
# m[0], m[1], m[2],
# g[0], g[1], g[2])
# + Style.BRIGHT + ' {:>6.1f} {:>6.1f} {:>6.1f}° |'.format(r, p, deg) + Style.RESET_ALL)
# time.sleep(0.50)
# print('-'*header)
# print(' uT: micro Tesla')
# print(' g: gravity')
# print('dps: degrees per second')
# print('')
#
# print(Fore.MAGENTA + 'acc: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(accel_x, accel_y, accel_z)\
# + Fore.YELLOW + ' \tmag: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(mag_x, mag_y, mag_z)\
# + Fore.CYAN + ' \tgyr: ({0:0.3f}, {1:0.3f}, {2:0.3f})'.format(gyro_x, gyro_y, gyro_z) + Style.RESET_ALL)
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')
def _one_meter(self, pid, callback):
'''
Threaded method for one_meter, one for each PID controller.
This will enabled the PID if not already enabled.
'''
_current_steps = pid.steps # get this quickly
_rate = Rate(20)
if not pid.enabled:
pid.enable()
pid.set_slew_rate(SlewRate.SLOWER)
pid.enable_slew(True)
# self._geo.steps_for_distance_cm
_distance_cm = 200.0 # should be 1m
_target_step_count = _distance_cm * self._geo.steps_per_cm
_target_steps = round(_current_steps + _target_step_count)
_closing_target_steps = _target_steps - self._geo.steps_per_rotation
# last wheel rotation
_final_target_step_count = _target_steps - (
1 * self._geo.steps_per_rotation)
_proposed_accel_range_cm = _distance_cm / 4.0
if _proposed_accel_range_cm * 2.0 >= self._accel_range_cm: # is the proposed range greater than the standard range?
# then we use standard acceleration/deceleration
self._log.info(
'using standard accel/decel range (compressed: {:5.2f}cm; standard: {:5.2f}cm)'
.format(_proposed_accel_range_cm, self._accel_range_cm))
_accel_range_cm = self._accel_range_cm
else: # otherwise compress to just one fourth of distance
self._log.info(
'using compressed accel/decel range (compressed: {:5.2f}cm; standard: {:5.2f}cm)'
.format(_proposed_accel_range_cm, self._accel_range_cm))
_accel_range_cm = _proposed_accel_range_cm
_accel_range_steps = round(_accel_range_cm * self._geo.steps_per_cm)
self._log.info(
'using accel/decel range of {:5.2f}cm, or {:d} steps.'.format(
_accel_range_cm, _accel_range_steps))
_accel_target_steps = _current_steps + _accel_range_steps # accelerate til this step count
_decel_target_steps = _target_steps - _accel_range_steps # step count when we begin to decelerate
self._log.info(Fore.YELLOW + 'begin one meter travel for {} motor accelerating from {:d} to {:d} steps, then cruise until {:d} steps, when we decelerate to {:d} steps and halt.'.format(\
pid.orientation.label, _current_steps, _accel_target_steps, _decel_target_steps, _target_steps))
_actually_do_this = True
if _actually_do_this:
# _accel_velocity = 0
# accelerate to cruising velocity ............................................
# self._rgbmatrix.show_color(Color.CYAN, Orientation.STBD)
self._log.info(
Fore.YELLOW +
'{} motor accelerating...'.format(pid.orientation.label))
while pid.steps < _accel_target_steps:
pid.velocity = self._cruising_velocity
_rate.wait()
# cruise until 3/4 of range ..................................................
self._log.info(Fore.YELLOW +
'{} motor reached cruising velocity...'.format(
pid.orientation.label))
# self._rgbmatrix.show_color(Color.GREEN, Orientation.STBD)
pid.velocity = self._cruising_velocity
while pid.steps < _decel_target_steps: # .....................................
_rate.wait()
# slow down until we reach 9/10 of range
# self._rgbmatrix.show_color(Color.YELLOW, Orientation.STBD)
pid.velocity = round(
(self._cruising_velocity + self._targeting_velocity) / 2.0)
while pid.steps < _decel_target_steps: # .....................................
_rate.wait()
# self._rgbmatrix.show_color(Color.ORANGE, Orientation.STBD)
self._log.info(
Fore.YELLOW +
'{} motor reached 9/10 of target, decelerating to targeting velocity...'
.format(pid.orientation.label))
pid.set_slew_rate(SlewRate.NORMAL)
# decelerate to targeting velocity when we get to one wheel revo of target ...
pid.velocity = self._targeting_velocity
while pid.steps < _closing_target_steps: # ...................................
_rate.wait()
# self._rgbmatrix.show_color(Color.RED, Orientation.STBD)
pid.velocity = self._targeting_velocity
while pid.steps < _target_steps: # ...........................................
# self._log.info(Fore.YELLOW + Style.DIM + '{:d} steps...'.format(pid.steps))
time.sleep(0.001)
pid.velocity = 0.0
# self._rgbmatrix.show_color(Color.BLACK, Orientation.STBD)
self._log.info(
Fore.YELLOW +
'{} motor stopping...'.format(pid.orientation.label))
time.sleep(1.0)
pid.set_slew_rate(SlewRate.NORMAL)
self._log.info(
Fore.YELLOW +
'executing {} callback...'.format(pid.orientation.label))
callback()
self._rgbmatrix.disable()
# pid.reset()
# pid.disable()
self._log.info(Fore.YELLOW +
'one meter on {} motor complete: {:d} of {:d} steps.'.
format(pid.orientation.label, pid.steps, _target_steps))
def __init__(self, config, message_bus, message_factory, level):
super().__init__()
self._log = Logger("clock", level)
if message_bus is None:
raise ValueError('null message bus argument.')
self._message_bus = message_bus
if message_factory is None:
raise ValueError('null message factory argument.')
self._message_factory = message_factory
if config is None:
raise ValueError('null configuration argument.')
_config = config['ros'].get('clock')
self._loop_freq_hz = _config.get('loop_freq_hz')
self._tock_modulo = _config.get('tock_modulo')
self._log.info('tock modulo: {:d}'.format(self._tock_modulo))
self._counter = itertools.count()
self._rate = Rate(self._loop_freq_hz)
# self._tick_type = type(Tick(None, Event.CLOCK_TICK, None))
# self._tock_type = type(Tock(None, Event.CLOCK_TOCK, None))
self._log.info('tick frequency: {:d}Hz'.format(self._loop_freq_hz))
self._log.info('tock frequency: {:d}Hz'.format(
round(self._loop_freq_hz / self._tock_modulo)))
self._enable_trim = _config.get('enable_trim')
self._log.info('enable clock trim.' if self.
_enable_trim else 'disable clock trim.')
self._pot = None
if self._enable_trim:
if SCALE_KP:
_min_pot = 0.2
_max_pot = 0.6
_kp = 0.0
_ki = 0.0
_kd = 0.00001
elif SCALE_KI:
_min_pot = 0.0
_max_pot = 0.01
_kp = 0.3333
_ki = 0.0
_kd = 0.00001
elif SCALE_KD:
_min_pot = 0.0
_max_pot = 0.01
_kp = 0.3333
_ki = 0.0
_kd = 0.0
else: # use these constants for the PID with no potentiometer control
_kp = 0.3333
_ki = 0.0
_kd = 0.00001
if (SCALE_KP or SCALE_KI or SCALE_KD):
try:
self._pot = Potentiometer(config, Level.WARN)
except Exception:
self._log.info(Fore.YELLOW + 'using mock potentiometer.')
self._pot = MockPotentiometer()
self._pot.set_output_limits(_min_pot, _max_pot)
else:
self._log.info('using fixed PID constants; no potentiometer.')
# initial constants
_setpoint = self._rate.get_period_ms()
self._log.info(Style.BRIGHT +
'initial PID setpoint: {:6.3f}'.format(_setpoint))
_min_output = None
_max_output = None
_sample_time = 0.05
self._pid = PID('clock', _kp, _ki, _kd, _min_output, _max_output,
_setpoint, _sample_time, Level.WARN)
self._log.info(Style.BRIGHT + 'trim enabled.')
else:
self._pid = None
self._log.info(Style.DIM + 'trim disabled.')
# .........................
self._thread = None
self._enabled = False
self._closed = False
self._last_time = dt.now()
self._log.info('ready.')
def run(self):
'''
This first disables the Pi's status LEDs, establishes the
message queue arbitrator, the controller, enables the set
of features, then starts the main OS loop.
'''
super(AbstractTask, self).run()
loop_count = 0
self._print_banner()
self._disable_leds = self._config['pi'].get('disable_leds')
if self._disable_leds:
# disable Pi LEDs since they may be distracting
self._set_pi_leds(False)
_main_loop_freq_hz = self._config['ros'].get('main_loop_freq_hz')
self._main_loop_rate = Rate(_main_loop_freq_hz)
# __enable_player = self._config['ros'].get('enable_player')
# if __enable_player:
# self._log.info('configuring sound player...')
# self._player = Player(Level.INFO)
# else:
# self._player = None
# i2c_slave_address = config['ros'].get('i2c_master').get('device_id') # i2c hex address of I2C slave device
# vl53l1x_available = True # self.get_property('features', 'vl53l1x')
# ultraborg_available = True # self.get_property('features', 'ultraborg')
# if vl53l1x_available and ultraborg_available:
# self._log.critical('starting scanner tool...')
# self._lidar = Lidar(self._config, Level.INFO)
# self._lidar.enable()
# else:
# self._log.critical('lidar scanner tool does not have necessary dependencies.')
# wait to stabilise features?
# _flask_enabled = self._config['flask'].get('enabled')
# if _flask_enabled:
# self._log.info('starting flask web server...')
# self.configure_web_server()
# else:
# self._log.info('not starting flask web server (suppressed from command line).')
# bluetooth gamepad controller
# if self._gamepad_enabled:
# self._connect_gamepad()
# _wait_for_button_press = self._config['ros'].get('wait_for_button_press')
# self._controller.set_standby(_wait_for_button_press)
# begin main loop ..............................
# self._log.info('enabling bno055 sensor...')
# self._bno055.enable()
# self._bumpers.enable()
# self._indicator = Indicator(Level.INFO)
# add indicator as message listener
# self._queue.add_listener(self._indicator)
# self._log.info(Fore.MAGENTA + 'enabling integrated front sensor...')
# self._ifs.enable()
# self._log.info('starting info thread...')
# self._info.start()
# self._log.info('starting blinky thread...')
# self._rgbmatrix.enable(DisplayType.RANDOM)
self._log.info('enabling features...')
for feature in self._features:
self._log.info('enabling feature {}...'.format(feature.name()))
feature.enable()
self._log.notice('Press Ctrl-C to exit.')
self._log.info('begin main os loop.\r')
# enable arbitrator tasks (normal functioning of robot)
# self._arbitrator.start()
_main_loop_counter = itertools.count()
self._active = True
while self._active:
# the timing of this loop is inconsequential; it exists solely as a keep-alive.
self._log.debug('[{:d}] main loop...'.format(
next(_main_loop_counter)))
self._main_loop_rate.wait()
if not self._closing:
self._log.warning('closing following loop...')
self.close()
def _loop(self, _enabled, _capturing):
'''
Captures an image, optionally writing it to the console in color,
then post-processing the array to enumerate the peak of matches
in a single linear array.
We don't necessarily process the whole image (it takes a long time),
processing from start_row to end_row only.
'''
self._start_time = dt.now()
self._column_summary = [0] * self._width
try:
# summarise elements on a column basis .........
self._motors.set_led_show_battery(False)
self._motors.set_led_color(Color.BLACK)
self._log.debug('starting image capture...')
with picamera.PiCamera() as camera:
with CaptureArray(camera, Level.INFO) as output:
camera.resolution = (self._width, self._height)
camera.iso = 800
camera.exposure_mode = 'auto'
# time.sleep(1.0)
if self._take_snapshot: # take JPEG image and save to file
_ts = dt.utcfromtimestamp(
dt.utcnow().timestamp()).isoformat().replace(
':', '_').replace('-', '_').replace('.', '_')
_filename = './snapshot-{}.jpg'.format(_ts)
self._log.info(
'writing snapshot to file: {}'.format(_filename))
camera.capture(_filename)
# output.truncate(0)
_rate = Rate(5) # 10Hz
while _enabled:
self._log.info(Fore.GREEN + Style.BRIGHT +
'looping snapshot...')
# capture an image
# camera.capture(output, 'rgb')
_use_video_port = False
for x in camera.capture_continuous(
output,
format='rgb',
use_video_port=_use_video_port):
image = output.array
if image is not None:
self._log.info(
'captured size {:d}x{:d} image.'.format(
image.shape[1], image.shape[0]))
else:
self._log.info('captured no image.')
output.truncate(0)
# self._process_image(image)
# self._log.set_suppress(True)
# while not _capturing:
# self._log.info(Fore.MAGENTA + Style.DIM + 'waiting...')
time.sleep(0.5)
# _rate.wait()
# _capturing = False
# self._log.info(Fore.CYAN + Style.DIM + 'loop end, repeating...')
except picamera.PiCameraMMALError:
self._log.error('could not get camera: in use by another process.')
except Exception:
self._log.error('error starting ros: {}'.format(
traceback.format_exc()))
finally:
self._log.set_suppress(False)
self._motors.set_led_show_battery(True)
self._log.info('finis.')
def test_motors():
global _port_motor, _stbd_motor, action_A, action_B
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.WARN)
_log.info('creating clock...')
_clock = Clock(_config, _message_bus, _message_factory, Level.WARN)
_motor_configurer = MotorConfigurer(_config, _clock, Level.INFO)
_motors = _motor_configurer.get_motors()
_motors.enable()
_i2c_scanner = I2CScanner(Level.WARN)
if ENABLE_MOTH:
if _i2c_scanner.has_address([0x18]):
_rgbmatrix = RgbMatrix(Level.WARN)
# _rgbmatrix.set_display_type(DisplayType.SOLID)
_moth = Moth(_config, None, Level.WARN)
else:
_log.warning('cannot enable moth: no IO Expander found.')
_moth = None
_pin_A = 16
_button_16 = Button(_pin_A, callback_method_A, Level.INFO)
_log.info(
Style.BRIGHT +
'press button A (connected to pin {:d}) to toggle or initiate action.'.
format(_pin_A))
_pin_B = 24
_button_24 = Button(_pin_B, callback_method_B, Level.INFO)
_log.info(Style.BRIGHT +
'press button B connected to pin {:d}) to exit.'.format(_pin_B))
_log.info('starting motors...')
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
if ENABLE_PORT:
_port_motor.enable()
if ENABLE_STBD:
_stbd_motor.enable()
_rot_ctrl = RotaryControl(_config, 0, 50, 2, Level.WARN) # min, max, step
_rate = Rate(5)
_step_limit = 2312
_velocity_stbd = 0.0
_velocity_port = 0.0
_start_time = dt.now()
_moth_port = 1.0
_moth_stbd = 1.0
_moth_offset = 0.6
_moth_bias = [0.0, 0.0]
_moth_fudge = 0.7
try:
action_A = True # if not using buttons at all set to True
action_B = True
while INFINITE or action_B or (_port_motor.steps < _step_limit
and _stbd_motor.steps < _step_limit):
if action_A:
action_A = False # trigger once
while action_B:
if USE_ROTARY_CONTROL:
_target_velocity = _rot_ctrl.read()
else:
_target_velocity = 30.0
# _power = _target_velocity / 100.0
_power = Motor.velocity_to_power(_target_velocity)
if ENABLE_MOTH and _moth:
_moth_bias = _moth.get_bias()
# _log.info(Fore.WHITE + Style.BRIGHT + 'port: {:5.2f}; stbd: {:5.2f}'.format(_moth_port, _moth_stbd))
# _rgbmatrix.show_hue(_moth_hue, Orientation.BOTH)
_orientation = _moth.get_orientation()
if _orientation is Orientation.PORT:
_moth_port = _moth_bias[0] * _moth_fudge
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
_rgbmatrix.show_color(Color.RED, Orientation.PORT)
elif _orientation is Orientation.STBD:
_moth_port = 1.0
_moth_stbd = _moth_bias[1] * _moth_fudge
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.GREEN,
Orientation.STBD)
else:
_moth_port = 1.0
_moth_stbd = 1.0
_rgbmatrix.show_color(Color.BLACK,
Orientation.PORT)
_rgbmatrix.show_color(Color.BLACK,
Orientation.STBD)
if ENABLE_STBD:
_stbd_motor.set_motor_power(_stbd_rotate * _power *
_moth_stbd)
_velocity_stbd = _stbd_motor.velocity
if ENABLE_PORT:
_port_motor.set_motor_power(_port_rotate * _power *
_moth_port)
_velocity_port = _port_motor.velocity
_log.info(Fore.YELLOW + 'power: {:5.2f}; bias: '.format(_power) \
+ Fore.RED + ' {:5.2f} '.format(_moth_bias[0]) + Fore.GREEN + '{:5.2f};'.format(_moth_bias[1]) \
+ Fore.BLACK + ' target velocity: {:5.2f};'.format(_target_velocity) \
+ Fore.CYAN + ' velocity: ' \
+ Fore.RED + ' {:5.2f} '.format(_velocity_port) + Fore.GREEN + ' {:5.2f}'.format(_velocity_stbd))
# _log.info(Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; \t'.format(_stbd_motor.get_current_power_level(), _power, _port_motor.steps) \
# + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps.'.format(_port_motor.get_current_power_level(), _power, _stbd_motor.steps))
_rate.wait()
action_B = True # reentry into B loop, waiting for A
_log.info('waiting for A button press...')
time.sleep(1.0)
# end wait loop ....................................................
if ENABLE_PORT:
_log.info('port motor: {:d} steps.'.format(_port_motor.steps))
if ENABLE_STBD:
_log.info('stbd motor: {:d} steps.'.format(_stbd_motor.steps))
except KeyboardInterrupt:
_log.info('Ctrl-C caught; exiting...')
except Exception as e:
_log.error('error: {}'.format(e))
finally:
close_motors(_log)
_elapsed_ms = round((dt.now() - _start_time).total_seconds() * 1000.0)
_log.info(Fore.YELLOW + 'complete: elapsed: {:d}ms'.format(_elapsed_ms))
def main():
_level = Level.INFO
_log = Logger('main', _level)
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
try:
_motors = Motors(_config, None, Level.WARN)
_pot = Potentiometer(_config, Level.WARN)
_pot.set_output_limits(0.0, 127.0)
# motor configuration: starboard, port or both?
_orientation = Orientation.BOTH
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid = PIDController(_config, _port_motor, level=Level.WARN)
_port_pid.enable()
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid = PIDController(_config, _stbd_motor, level=Level.WARN)
_stbd_pid.enable()
ROTATE = True
_rotate = -1.0 if ROTATE else 1.0
# sys.exit(0)
_stbd_velocity = 0.0
_port_velocity = 0.0
_step = 0.5
_min = 0.0
_max = 70.0
_rate = Rate(10)
try:
# for _value in numpy.arange(_min, _max, _step):
while True:
# update RGB LED
# _pot.set_rgb(_pot.get_value())
_value = 127.0 - _pot.get_scaled_value(True)
if _value > 125.0:
_value = 127.0
_velocity = Gamepad.convert_range(_value)
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.setpoint = _velocity * 100.0
_port_velocity = _port_pid.setpoint
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.setpoint = _rotate * _velocity * 100.0
_stbd_velocity = _stbd_pid.setpoint
_log.info(Fore.WHITE + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \
+ Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity))
_rate.wait()
_log.info(Fore.YELLOW + 'resting...')
time.sleep(10.0)
# for _value in numpy.arange(_min, _max, _step):
while True:
# update RGB LED
# _pot.set_rgb(_pot.get_value())
_value = 127.0 - _pot.get_scaled_value(True)
if _value > 125.0:
_value = 127.0
_velocity = Gamepad.convert_range(_value)
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.setpoint = _rotate * _velocity * 100.0
_port_velocity = _port_pid.setpoint
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.setpoint = _velocity * 100.0
_stbd_velocity = _stbd_pid.setpoint
_log.info(Fore.MAGENTA + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \
+ Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity))
_rate.wait()
_log.info(Fore.YELLOW + 'end of cruising.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'PID test complete.')
finally:
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.disable()
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.disable()
_motors.brake()
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}\n{}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
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.')
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 = None
_e_pitch = None
_e_roll = None
_e_yaw = None
_q_heading = None
_q_pitch = None
_q_roll = None
_q_yaw = None
_quat_w = None
_quat_x = None
_quat_y = None
_quat_z = None
count = 0
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
_rate = Rate(20) # 20Hz
while not self._closed:
if self._enabled:
_status = self._bno055.calibration_status
# status: sys, gyro, accel, mag
_sys_calibrated = (_status[0] >= 3)
_gyro_calibrated = (_status[1] >= 3)
_accelerometer_calibrated = (_status[2] >= 3)
_magnetometer_calibrated = (_status[3] >= 3)
# "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
self._is_calibrated = _gyro_calibrated and _magnetometer_calibrated
# self._is_calibrated = self._bno055.calibrated
if self._is_calibrated:
self._log.debug('calibrated: s{}-g{}-a{}-m{}.'.format(
_status[0], _status[1], _status[2], _status[3]))
else:
self._log.debug('not calibrated: s{}-g{}-a{}-m{}.'.format(
_status[0], _status[1], _status[2], _status[3]))
# 1. Absolute Orientation (Euler Vector, 100Hz) Three axis orientation data based on a 360° sphere ...........................
_euler = self._bno055.euler
if _euler[0] is not None:
_e_heading = _euler[0]
if _euler[1] is not None:
_e_pitch = -1.0 * _euler[1]
if _euler[2] is not None:
_e_roll = _euler[2]
# 2. Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation ................
_quat = self._bno055.quaternion
if _quat is not None:
if _quat[0] is not None:
_quat_w = _quat[0]
if _quat[1] is not None:
_quat_x = _quat[1]
if _quat[2] is not None:
_quat_y = _quat[2]
if _quat[3] is not None:
_quat_z = _quat[3]
_sys_calibrated = True # override, just pay attention to values
if _sys_calibrated \
and ( None not in (_e_heading, _e_pitch, _e_roll ) ) \
and ( None not in (_quat_w, _quat_x, _quat_y, _quat_z ) ):
# _q_euler = self._convert_to_euler(_quat_w, _quat_x, _quat_y, _quat_z)
_q = self._quaternion_to_euler_angle(
_quat_w, _quat_x, _quat_y, _quat_z)
_q_yaw_pitch_roll = _q.yaw_pitch_roll
_q_heading = _q.degrees
_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 ............................................................
if BNO055._in_range(_e_heading, _q_heading):
self._heading = _q_heading + self._heading_trim
self._log.debug(' ' + Fore.MAGENTA + Style.BRIGHT + 'eh: \t {:>5.4f}° (cal); '.format(_e_heading) \
+ Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw))
elif self._quaternion_accept: # if true, we permit Quaternion once we've achieved calibration
self._heading = _e_heading + self._heading_trim
self._log.debug(' ' + Fore.CYAN + Style.NORMAL + 'eh: \t {:>5.4f}° (qua); '.format(_e_heading) \
+ Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw))
else:
self._log.debug(' ' + Fore.CYAN + Style.DIM + 'eh: \t {:>5.4f}° (non); '.format(_e_heading) \
+ Fore.BLACK + Style.NORMAL + '\t qh={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw))
self._heading = None
# pitch ..............................................................
if BNO055._in_range(_e_pitch, _q_pitch):
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 BNO055._in_range(_e_roll, _q_roll):
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
# 3. Angular Velocity Vector (20Hz) Three axis of 'rotation speed' in rad/s ..................................................
# _gyro = self._bno055.gyro
# self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 0: \t{:>5.4f} rad/sec'.format(_gyro[0]))
# self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 1: \t{:>5.4f} rad/sec'.format(_gyro[1]))
# self._log.info(' ' + Fore.YELLOW + Style.BRIGHT + 'gyroscope 2: \t{:>5.4f} rad/sec'.format(_gyro[2]))
# 4. Acceleration Vector (100Hz) Three axis of acceleration (gravity + linear motion) in m/s^2 ...............................
# _accel = self._bno055.acceleration
# self._log.info(' ' + Fore.MAGENTA + Style.BRIGHT + 'accelerometer (m/s^2): {}'.format(_accel))
# 5. Magnetic Field Strength Vector (100Hz) Three axis of magnetic field sensing in micro Tesla (uT) .........................
# _magneto = self._bno055.magnetic
# _mag_x = _magneto[0] * 100.0
# _mag_y = _magneto[1] * 100.0
# _mag_z = _magneto[2] * 100.0
# _simple_heading = ( 180.0 * math.atan2(_mag_y,_mag_x)/math.pi )
# if _simple_heading < 0:
# _simple_heading += 360
# self._log.info(Fore.MAGENTA + Style.BRIGHT + 'simple:\t{:>5.2f}° \t{:>5.2f}°'.format(_simple_heading, _simple_heading - self._heading))
# else:
# self._log.info(Fore.GREEN + Style.BRIGHT + 'simple:\t{:>5.2f}° \t{:>5.2f}°'.format(_simple_heading, _simple_heading - self._heading))
# r, p, h = self.getOrientation(_accel, _magneto)
# deg = self.to_degrees(h)
# # 2pi = 6.283185307
# if h > self._h_max:
# self._h_max = h
# if h < self._h_min:
# self._h_min = h
# self._log.debug(Fore.CYAN + Style.BRIGHT + 'pitch/roll/heading: {:>6.3f};\t{:>6.3f}'.format(p, r) \
# + Fore.MAGENTA + '\t{:>6.3f}rad\t{:>6.3f}°'.format(h, deg) + Fore.BLACK + '\t({:>6.3f}->{:>6.3f})'.format(self._h_min, self._h_max))
# 6. Linear Acceleration Vector (100Hz) Three axis of linear acceleration data (acceleration minus gravity) in m/s^2 .........
# self._log.info(' ' + Fore.BLUE + Style.BRIGHT + 'linear acceleration (m/s^2): {}'.format(self._bno055.linear_acceleration))
# 7. Gravity Vector (100Hz) Three axis of gravitational acceleration (minus any movement) in m/s^2 ...........................
# self._log.info(' ' + Fore.WHITE + Style.BRIGHT + 'gravity (m/s^2): {}\n'.format(self._bno055.gravity))
# 8. Temperature (1Hz) Ambient temperature in degrees celsius ................................................................
# self._log.info(' ' + Fore.MAGENTA + Style.BRIGHT + 'temperature: {} degrees C'.format(self._bno055.temperature))
# time.sleep(self._loop_delay_sec)
_rate.wait()
else:
self._log.info('disabled loop.')
time.sleep(10)
count += 1
self._log.debug('read loop ended.')