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():
_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():
_level = Level.INFO
_log = Logger('main', _level)
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_rot_min = 0
_rot_max = 100
_rot_step = 5
_rot_ctrl = RotaryControl(_config, _rot_min, _rot_max, _rot_step,
Level.WARN)
_pot_min = 0.0
_pot_max = 0.00030
_pot = Potentiometer(_config, Level.INFO)
_pot.set_output_limits(_pot_min, _pot_max)
try:
_motors = Motors(_config, None, _level)
# _port_motor = _motors.get_motor(Orientation.PORT)
# _port_pid = PIDController(_config, _port_motor, level=Level.DEBUG)
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid = PIDController(_config, _stbd_motor, level=Level.DEBUG)
# sys.exit(0)
_max_velocity = 30.0
# _port_pid.enable()
_stbd_pid.enable()
try:
_log.info(Fore.YELLOW + 'accelerating...')
_read_value = _rot_ctrl.read()
_velocity = _read_value / 100.0
_log.info(Fore.YELLOW + 'value: {}'.format(_read_value))
_stbd_pid.setpoint = 10.0
# for _velocity in numpy.arange(0.0, _max_velocity, 0.3):
# _log.info(Fore.YELLOW + '_velocity={:>5.2f}.'.format(_velocity))
# _stbd_pid.setpoint = _velocity
# _log.info(Fore.GREEN + 'STBD setpoint={:>5.2f}.'.format(_stbd_pid.setpoint))
# _port_pid.setpoint = _velocity
# _log.info(Fore.RED + 'PORT setpoint={:>5.2f}.'.format(_port_pid.setpoint))
# time.sleep(1.0)
# ..........................................
_log.info(Fore.YELLOW + 'cruising...')
time.sleep(10.0)
_log.info(Fore.YELLOW + 'end of cruising.')
# _log.info(Fore.YELLOW + 'decelerating...')
# for _velocity in numpy.arange(_max_velocity, 0.0, -0.25):
# _log.info(Fore.YELLOW + '_velocity={:>5.2f}.'.format(_velocity))
# _port_pid.setpoint = _velocity
# _stbd_pid.setpoint = _velocity
# _log.info(Fore.GREEN + 'STBD setpoint={:>5.2f}.'.format(_stbd_pid.setpoint))
# _log.info(Fore.RED + 'PORT setpoint={:>5.2f}.'.format(_port_pid.setpoint))
# time.sleep(1.0)
# _log.info(Fore.YELLOW + 'stopped...')
_stbd_pid.setpoint = 0.0
time.sleep(3.0)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'PID test complete.')
finally:
# _port_pid.disable()
_stbd_pid.disable()
_motors.brake()
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 main():
global test_triggered, test_enabled
# initial states
test_triggered = False
test_enabled = True
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_pin_A = 12
_button_24 = 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))
try:
# .........................................
_motors = Motors(_config, None, Level.INFO)
_pot = Potentiometer(_config, Level.WARN)
_pot_min = 0.0000
_pot_max = 0.1500
_pot.set_output_limits(_pot_min, _pot_max)
_limit = 90.0
# _min = -127
_stbd_setpoint = 0.0
_port_setpoint = 0.0
_scaled_value = 0.0
_value = 0.0
_port_pid = None
_stbd_pid = None
# rotary controller
_min = 90
_max = 0
_step = 5
_rotary_ctrl = RotaryControl(_config, _min, _max, _step, Level.INFO)
# configure motors/PID controllers
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid = PIDController(_config, _port_motor, level=Level.INFO)
_port_pid.set_limit(_limit)
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_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.INFO)
_stbd_pid.set_limit(_limit)
_stbd_pid.enable()
# sys.exit(0)
_fixed_value = 15.0 # fixed setpoint value
DRY_RUN = False
USE_POTENTIOMETER = False
USE_ROTARY_ENCODER = False
ANYTHING_ELSE = False
try:
while test_enabled:
if test_triggered:
_log.info(Fore.BLUE + Style.BRIGHT + 'action A.')
test_triggered = False # trigger once
rotate_in_place(_rotary_ctrl, _port_pid, _stbd_pid)
test_enabled = False # quit after action
time.sleep(1.0)
# and we now return to our regularly scheduled broadcast...
_kp = _pot.get_scaled_value(True)
_stbd_pid._pid.kp = _kp
_log.info(Fore.YELLOW + 'waiting for button A...;' +
Style.BRIGHT +
' kp: {:8.5f}'.format(_stbd_pid._pid.kp))
time.sleep(1.0)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'Ctrl-C caught: closing...')
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()
if _rotary_ctrl:
_log.info('resetting rotary encoder...')
_rotary_ctrl.reset()
time.sleep(1.0)
_log.info('complete.')
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT +
'error in test: {}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'finally.')
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 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.')
def main():
global action_A, action_B
# initial states
action_A = False
action_B = True
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
_pin_A = 12
_button_12 = 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))
try:
_log.info('creating message factory...')
_message_factory = MessageFactory(Level.INFO)
_log.info('creating message bus...')
_message_bus = MessageBus(Level.DEBUG)
_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()
_limit = 90.0
_stbd_setpoint = 0.0
_port_setpoint = 0.0
_scaled_value = 0.0
_value = 0.0
_port_pid_ctrl = None
_stbd_pid_ctrl = None
# rotary controller
_min_rot = 0
_max_rot = 30
_step_rot = 1
_rotary_ctrl = RotaryControl(_config, _min_rot, _max_rot, _step_rot,
Level.WARN)
# configure motors/PID controllers
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid_ctrl = PIDController(_config,
_clock,
_port_motor,
level=Level.INFO)
_port_pid_ctrl.limit = _limit
_port_pid_ctrl.enable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid_ctrl = PIDController(_config,
_clock,
_stbd_motor,
level=Level.INFO)
_stbd_pid_ctrl.limit = _limit
_stbd_pid_ctrl.enable()
_max_pot = 1.0
_min_pot = 0.0
_pot = Potentiometer(_config, Level.WARN)
_pot.set_output_limits(_min_pot, _max_pot)
_motors.enable()
try:
while action_B:
if action_A:
_log.info(Fore.BLUE + Style.BRIGHT + 'action A.')
action_A = False # trigger once
_stbd_setpoint = 0.0
_port_setpoint = 0.0
while action_B:
# set PID constant
_scaled_value = _pot.get_scaled_value(True)
# _log.info(Style.DIM + 'PID TUNER kp: {:8.5f} ================ '.format(_scaled_value))
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.kp = _scaled_value
_stbd_pid_ctrl.kp = _scaled_value
_value = _rotary_ctrl.read()
# _log.info(Fore.BLUE + Style.BRIGHT + 'rotary value: {:<5.2f}'.format(_value))
# if _value > -2.0 and _value < 2.0: # hysteresis?
# _value = 0.0
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.setpoint = ROTATE_VALUE * _value #* 100.0
_port_setpoint = _port_pid_ctrl.setpoint
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.setpoint = _value #* 100.0
_stbd_setpoint = _stbd_pid_ctrl.setpoint
# _log.info(Fore.MAGENTA + 'rotary value: {:<5.2f}; setpoint: '.format(_value) \
# + Fore.RED + ' {:5.2f};'.format(_port_setpoint) + Fore.GREEN + ' {:5.2f};'.format(_stbd_setpoint) \
# + Fore.WHITE + ' action_B: {}'.format(action_B))
# end of loop
action_B = False # quit after action
time.sleep(1.0)
# and we now return to our regularly scheduled broadcast...
else:
pass
_log.info(Fore.BLACK + Style.BRIGHT + 'waiting on button A...')
time.sleep(0.5)
_log.info(Fore.YELLOW + 'end of test.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'Ctrl-C caught: closing...')
finally:
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT:
_port_pid_ctrl.disable()
if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.STBD:
_stbd_pid_ctrl.disable()
_motors.brake()
if _rotary_ctrl:
_log.info('resetting rotary encoder...')
_rotary_ctrl.reset()
time.sleep(1.0)
_log.info('complete.')
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT +
'error in test: {}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'finally.')