def main():
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_queue = MessageQueue(Level.INFO)
_ifs = IntegratedFrontSensor(_config, _queue, Level.INFO)
_ifs.enable()
_motors = Motors(_config, None, None, Level.INFO)
print(Fore.RED + 'avoiding PORT...' + Style.RESET_ALL)
_avoid = Behaviours(_motors,_ifs, Level.INFO)
_avoid.avoid(Orientation.PORT)
time.sleep(1.0)
# print(Fore.BLUE + 'avoiding CNTR...' + Style.RESET_ALL)
# _avoid = AvoidBehaviour(_motors,_ifs, Level.INFO)
# _avoid.avoid(Orientation.CNTR)
# time.sleep(1.0)
# print(Fore.GREEN + 'avoiding STBD...' + Style.RESET_ALL)
# _avoid = AvoidBehaviour(_motors,_ifs, Level.INFO)
# _avoid.avoid(Orientation.STBD)
print(Fore.YELLOW + 'complete: closing...' + Style.RESET_ALL)
_ifs.disable()
_ifs.close()
_motors.close()
print(Fore.YELLOW + 'closed.' + Style.RESET_ALL)
sys.exit(0)
except KeyboardInterrupt:
print('test complete')
sys.exit(0)
except Exception as e:
print(Fore.RED + Style.BRIGHT + 'error in PID controller: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
def main():
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_queue = MessageQueue(Level.INFO)
_ifs = IntegratedFrontSensor(_config, _queue, Level.INFO)
_motors = Motors(_config, None, None, Level.INFO)
# configure Controller and Arbitrator
_controller = Controller(_config, _ifs, _motors, _callback_shutdown,
Level.INFO)
_arbitrator = Arbitrator(_config, _queue, _controller, Level.WARN)
print(Fore.RED + 'roam...' + Style.RESET_ALL)
_ifs.enable()
_arbitrator.start()
# _controller.enable()
_roam = RoamBehaviour(_motors, Level.INFO)
_roam.roam()
while _roam.is_roaming():
print(Fore.BLACK + Style.DIM + '(roaming)' + Style.RESET_ALL)
time.sleep(1.0)
print(Fore.YELLOW + 'complete: closing...' + Style.RESET_ALL)
_motors.close()
print(Fore.YELLOW + 'closed.' + Style.RESET_ALL)
sys.exit(0)
except KeyboardInterrupt:
print('test complete')
sys.exit(0)
except Exception as e:
print(Fore.RED + Style.BRIGHT +
'error in PID controller: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
def main():
try:
# signal.signal(signal.SIGINT, signal_handler)
_wheel_circumference_mm = 218
_forward_steps_per_rotation = 494
_forward_steps = get_forward_steps()
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_queue = MessageQueue(Level.INFO)
_indicator = Indicator(Level.INFO)
_compass = Compass(_config, _queue, _indicator, Level.INFO)
_compass.enable()
print(Fore.CYAN + Style.BRIGHT +
'wave robot in the air until calibrated.' + Style.RESET_ALL)
while not _compass.is_calibrated():
time.sleep(0.33)
print(
Fore.CYAN + Style.BRIGHT +
'CALIBRATED. xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx '
+ Style.RESET_ALL)
time.sleep(5.0)
_motors = Motors(_config, None, None, Level.INFO)
# _port_motor = _motors.get_motor(Orientation.PORT)
# _stbd_motor = _motors.get_motor(Orientation.STBD)
_heading = 0.0 # due south
_spin(_motors, Rotation.CLOCKWISE,
lambda: not has_heading(_compass, _heading))
print(Fore.CYAN + Style.BRIGHT + 'test complete.' + Style.RESET_ALL)
except KeyboardInterrupt:
_stbd_motor.halt()
quit()
except Exception as e:
print(Fore.RED + Style.BRIGHT +
'error in PID controller: {}'.format(e) + Style.RESET_ALL)
traceback.print_exc(file=sys.stdout)
finally:
print(Fore.YELLOW + Style.BRIGHT + 'C. finally.' + Style.RESET_ALL)
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 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():
'''
494 encoder steps per rotation (maybe 493)
218mm wheel circumference
1 wheel rotation = 218mm
2262 steps per meter
2262 steps per second = 1 m/sec
2262 steps per second = 100 cm/sec
'''
STEPS_PER_METER = 2262
_wheel_circumference_mm = 218
_forward_steps_per_rotation = 494
_reverse_steps_per_rotation = 494
signal.signal(signal.SIGINT, signal_handler)
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_speed = 70.0
_motors = Motors(_config, None, None, Level.DEBUG)
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
_motors.enable()
_rotations = 3
forward_test = True
if forward_test:
#_forward_steps_per_rotation * _rotations
_forward_steps = int(1.0 * STEPS_PER_METER)
print(Fore.YELLOW + Style.BRIGHT + 'starting forward motor test for steps: {:d}.'.format(_forward_steps) + Style.RESET_ALL)
while not _motors.ahead_for_steps(_speed, _speed, _forward_steps, _forward_steps):
time.sleep(0.1)
time.sleep(2.0)
# _speed = -1 * _speed
# while not _motors.ahead_for_steps(_speed, _speed, _forward_steps, _forward_steps):
# time.sleep(0.1)
print(Fore.YELLOW + Style.BRIGHT + 'motor test complete; intended: {:d}; actual steps: {:d}/{:d}.'.format(\
_forward_steps, _port_motor.get_steps(), _stbd_motor.get_steps()) + Style.RESET_ALL)
else:
_reverse_steps = _reverse_steps_per_rotation * _rotations
print(Fore.YELLOW + Style.BRIGHT + 'starting reverse motor test for steps: {:d}.'.format(_reverse_steps) + Style.RESET_ALL)
# _stbd_motor.ahead_for_steps(-30.0, _reverse_steps)
# _stbd_motor.stop()
# print(Fore.YELLOW + Style.BRIGHT + 'motor test complete; intended: {:d}; actual steps: {:d}.'.format(\
# _reverse_steps, _stbd_motor.get_steps()) + Style.RESET_ALL)
except KeyboardInterrupt:
print('Ctrl-C caught, exiting...')
finally:
if _motors:
_motors.brake()
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, level):
super().__init__()
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
self._log = Logger("gamepad-demo", level)
self._log.heading('gamepad-demo', 'Configuring Gamepad...', None)
self._config = _config['ros'].get('gamepad_demo')
self._enable_ifs = self._config.get('enable_ifs')
self._enable_compass = self._config.get('enable_compass')
self._enable_indicator = self._config.get('enable_indicator')
self._message_factory = MessageFactory(level)
self._motors = Motors(_config, None, Level.INFO)
# self._motor_controller = SimpleMotorController(self._motors, Level.INFO)
self._pid_motor_ctrl = PIDMotorController(_config, self._motors,
Level.INFO)
# i2c scanner, let's us know if certain devices are available
_i2c_scanner = I2CScanner(Level.WARN)
_addresses = _i2c_scanner.get_int_addresses()
ltr559_available = (0x23 in _addresses)
'''
Availability of displays:
The 5x5 RGB Matrix is at 0x74 for port, 0x77 for starboard.
The 11x7 LED matrix is at 0x75 for starboard, 0x77 for port. The latter
conflicts with the RGB LED matrix, so both cannot be used simultaneously.
We check for either the 0x74 address to see if RGB Matrix displays are
used, OR for 0x75 to assume a pair of 11x7 Matrix displays are being used.
'''
# rgbmatrix5x5_stbd_available = ( 0x74 in _addresses ) # not used yet
# matrix11x7_stbd_available = ( 0x75 in _addresses ) # used as camera lighting
matrix11x7_stbd_available = False
# self._blob = BlobSensor(_config, self._motors, Level.INFO)
self._blob = None
self._lux = Lux(Level.INFO) if ltr559_available else None
self._video = None
# self._video = Video(_config, self._lux, matrix11x7_stbd_available, Level.INFO)
self._message_bus = MessageBus(Level.INFO)
# in this application the gamepad controller is the message queue
# self._queue = MessageQueue(self._message_factory, Level.INFO)
self._clock = Clock(_config, self._message_bus, self._message_factory,
Level.INFO)
# attempt to find the gamepad
self._gamepad = Gamepad(_config, self._message_bus,
self._message_factory, Level.INFO)
# if self._enable_indicator:
# self._indicator = Indicator(Level.INFO)
# if self._enable_compass:
# self._compass = Compass(_config, self._queue, self._indicator, Level.INFO)
# self._video.set_compass(self._compass)
_enable_battery_check = False
if _enable_battery_check:
self._log.info('starting battery check thread...')
self._battery_check = BatteryCheck(_config, self._queue,
self._message_factory,
Level.INFO)
else:
self._battery_check = None
if self._enable_ifs:
self._log.info('integrated front sensor enabled.')
self._ifs = IntegratedFrontSensor(_config, self._clock,
self._message_bus,
self._message_factory,
Level.INFO)
# add indicator as message consumer
if self._enable_indicator:
self._queue.add_consumer(self._indicator)
else:
self._ifs = None
self._log.info('integrated front sensor disabled.')
# self._ctrl = GamepadController(_config, self._queue, self._pid_motor_ctrl, self._ifs, self._video, self._blob, matrix11x7_stbd_available, Level.INFO, self._close_demo_callback)
self._ctrl = GamepadController(_config, self._message_bus,
self._pid_motor_ctrl, self._ifs,
self._video, self._blob,
matrix11x7_stbd_available, Level.INFO,
self._close_demo_callback)
self._message_bus.add_handler(Message, self._ctrl.handle_message)
self._enabled = False
self._log.info('connecting gamepad...')
self._gamepad.connect()
self._log.info('ready.')
#!/usr/bin/env python3 # -*- coding: utf-8 -*- # # Copyright 2020 by Murray Altheim. All rights reserved. This file is part of # the Robot OS project and is released under the "Apache Licence, Version 2.0". # Please see the LICENSE file included as part of this package. # # author: Murray Altheim # created: 2020-03-30 # modified: 2020-08-21 # # Quickly halts the motors if they're running. # import time from lib.logger import Level from lib.config_loader import ConfigLoader from lib.motors import Motors _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _motors = Motors(_config, None, Level.INFO) _motors.halt() _motors.close() #EOF
def configure(self):
'''
Import the ThunderBorg library, then configure and return the Motors.
'''
self._log.info('configure thunderborg & motors...')
try:
self._log.info('importing ThunderBorg...')
import lib.ThunderBorg3 as ThunderBorg
self._log.info('successfully imported ThunderBorg.')
TB = ThunderBorg.ThunderBorg(Level.INFO) # create a new ThunderBorg object
TB.Init() # set the board up (checks the board is connected)
self._log.info('successfully instantiated ThunderBorg.')
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
self._log.error('No ThunderBorg found, check you are attached :)')
else:
self._log.error('No ThunderBorg at address %02X, but we did find boards:' % (TB.i2cAddress))
for board in boards:
self._log.info(' %02X (%d)' % (board, board))
self._log.error('If you need to change the I²C address change the setup line so it is correct, e.g. TB.i2cAddress = 0x{}'.format(boards[0]))
sys.exit(1)
TB.SetLedShowBattery(True)
# initialise ThunderBorg ...........................
self._log.debug('getting battery reading...')
# get battery voltage to determine max motor power
# could be: Makita 12V or 18V power tool battery, or 12V line supply
voltage_in = TB.GetBatteryReading()
if voltage_in is None:
raise OSError('cannot continue: cannot read battery voltage.')
self._log.info('voltage in: {:>5.2f}V'.format(voltage_in))
# voltage_in = 20.5
# maximum motor voltage
voltage_out = 9.0
self._log.info('voltage out: {:>5.2f}V'.format(voltage_out))
if voltage_in < voltage_out:
raise OSError('cannot continue: battery voltage too low ({:>5.2f}V).'.format(voltage_in))
# Setup the power limits
if voltage_out > voltage_in:
_max_power_ratio = 1.0
else:
_max_power_ratio = voltage_out / float(voltage_in)
# convert float to ratio format
self._log.info('battery level: {:>5.2f}V; motor voltage: {:>5.2f}V; maximum power ratio: {}'.format(voltage_in, voltage_out, \
str(Fraction(_max_power_ratio).limit_denominator(max_denominator=20)).replace('/',':')))
except Exception as e:
self._log.error('unable to import ThunderBorg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
# now import motors
from lib.motors import Motors
try:
self._log.info('getting raspberry pi...')
self._log.info('configuring motors...')
self._motors = Motors(self._config, TB, Level.INFO)
self._motors.get_motor(Orientation.PORT).set_max_power_ratio(_max_power_ratio)
self._motors.get_motor(Orientation.STBD).set_max_power_ratio(_max_power_ratio)
return self._motors
except OSError as oe:
self._log.error('failed to configure motors: {}'.format(oe))
# sys.stderr = DevNull()
sys.exit(1)
def main():
'''
494 encoder steps per rotation (maybe 493)
218mm wheel circumference
1 wheel rotation = 218mm
2262 steps per meter
2262 steps per second = 1 m/sec
2262 steps per second = 100 cm/sec
'''
_wheel_circumference_mm = 218
_forward_steps_per_rotation = 494
_reverse_steps_per_rotation = 494
signal.signal(signal.SIGINT, signal_handler)
try:
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_port_speed = 0.0
_stbdt_speed = 50.0
_port_steps = 0
_stbd_steps = 0
_motors = Motors(_config, None, None, Level.INFO)
# _motors.step(_port_speed, _stbd_speed, _port_steps, _stbd_steps):
_speed = 70.0
_port_motor = _motors.get_motor(Orientation.PORT)
_stbd_motor = _motors.get_motor(Orientation.STBD)
_rotations = 4
forward_test = True
if forward_test:
_forward_steps = _forward_steps_per_rotation * _rotations
print(Fore.YELLOW + Style.BRIGHT +
'starting forward motor test for steps: {:d}.'.format(
_forward_steps) + Style.RESET_ALL)
# _port_motor.ahead_for_steps(_speed, _forward_steps)
# _stbd_motor.ahead_for_steps(_speed, _forward_steps)
# _port_motor.stop()
# _stbd_motor.stop()
_motors.ahead_for_steps(_speed, _speed, _forward_steps,
_forward_steps)
_motors.stop()
print(Fore.YELLOW + Style.BRIGHT + 'motor test complete; intended: {:d}; actual port steps: {:d}, stbd steps: {:d}'.format(\
_forward_steps, _port_motor.get_steps(), _stbd_motor.get_steps()) + Style.RESET_ALL)
else:
_reverse_steps = _reverse_steps_per_rotation * _rotations
print(Fore.YELLOW + Style.BRIGHT +
'starting reverse motor test for steps: {:d}.'.format(
_reverse_steps) + Style.RESET_ALL)
_stbd_motor.ahead_for_steps(-30.0, _reverse_steps)
_stbd_motor.stop()
print(Fore.YELLOW + Style.BRIGHT +
'motor test complete; intended: {:d}; actual steps: {:d}.'.
format(_reverse_steps, _stbd_motor.get_steps()) +
Style.RESET_ALL)
except KeyboardInterrupt:
print('Ctrl-C caught, exiting...')
finally:
_motors.brake()
class MotorConfigurer():
'''
Configures the ThunderBorg motor controller for a pair of motors.
:param config: the application configuration
:param clock: the 'system' clock providing timing for velocity calculation
:param enable_mock: if set True a failure to instantiate the ThunderBorg will instead use a mock
:param level: the logging level
'''
def __init__(self, config, clock, enable_mock=False, level=Level.INFO):
self._log = Logger("motor-conf", level)
if config is None:
raise ValueError('null configuration argument.')
if clock is None:
raise ValueError('null clock argument.')
self._config = config
# Import the ThunderBorg library, then configure and return the Motors.
self._log.info('configure thunderborg & motors...')
try:
_i2c_scanner = I2CScanner(Level.WARN)
_has_thunderborg = _i2c_scanner.has_address([THUNDERBORG_ADDRESS])
if _has_thunderborg:
self._log.info('importing ThunderBorg...')
import lib.ThunderBorg3 as ThunderBorg
self._log.info('successfully imported ThunderBorg.')
else:
self._log.info('importing mock ThunderBorg...')
import mock.thunderborg as ThunderBorg
self._log.info('successfully imported mock ThunderBorg.')
TB = ThunderBorg.ThunderBorg(Level.INFO) # create a new ThunderBorg object
TB.Init() # set the board up (checks the board is connected)
self._log.info('successfully instantiated ThunderBorg.')
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
self._log.error('No ThunderBorg found, check you are attached :)')
else:
self._log.error('No ThunderBorg at address %02X, but we did find boards:' % (TB.i2cAddress))
for board in boards:
self._log.info(' %02X (%d)' % (board, board))
self._log.error('If you need to change the I²C address change the setup line so it is correct, e.g. TB.i2cAddress = 0x{}'.format(boards[0]))
raise Exception('unable to instantiate ThunderBorg [1].')
# sys.exit(1)
TB.SetLedShowBattery(True)
# initialise ThunderBorg ...........................
self._log.debug('getting battery reading...')
# get battery voltage to determine max motor power
# could be: Makita 12V or 18V power tool battery, or 12V line supply
voltage_in = TB.GetBatteryReading()
if voltage_in is None:
raise OSError('cannot continue: cannot read battery voltage.')
self._log.info('voltage in: {:>5.2f}V'.format(voltage_in))
# voltage_in = 20.5
# maximum motor voltage
voltage_out = 9.0
self._log.info('voltage out: {:>5.2f}V'.format(voltage_out))
if voltage_in < voltage_out:
raise OSError('cannot continue: battery voltage too low ({:>5.2f}V).'.format(voltage_in))
# Setup the power limits
if voltage_out > voltage_in:
_max_power_ratio = 1.0
else:
_max_power_ratio = voltage_out / float(voltage_in)
# convert float to ratio format
self._log.info('battery level: {:>5.2f}V; motor voltage: {:>5.2f}V; maximum power ratio: {}'.format(voltage_in, voltage_out, \
str(Fraction(_max_power_ratio).limit_denominator(max_denominator=20)).replace('/',':')))
except OSError as e:
if enable_mock:
self._log.info('using mock ThunderBorg.')
import mock.thunderborg as ThunderBorg
else:
self._log.error('unable to import ThunderBorg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
raise Exception('unable to instantiate ThunderBorg [2].')
except Exception as e:
if enable_mock:
self._log.info('using mock ThunderBorg.')
import mock.thunderborg as ThunderBorg
else:
self._log.error('unable to import ThunderBorg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
raise Exception('unable to instantiate ThunderBorg [2].')
# sys.exit(1)
# now import motors
from lib.motors import Motors
try:
self._log.info('getting raspberry pi...')
self._log.info('configuring motors...')
self._motors = Motors(self._config, clock, TB, Level.INFO)
self._motors.get_motor(Orientation.PORT).set_max_power_ratio(_max_power_ratio)
self._motors.get_motor(Orientation.STBD).set_max_power_ratio(_max_power_ratio)
except OSError as oe:
self._log.error('failed to configure motors: {}'.format(oe))
self._motors = None
# sys.stderr = DevNull()
raise Exception('unable to instantiate ThunderBorg [3].')
# sys.exit(1)
self._log.info('ready.')
# ..........................................................................
def get_motors(self):
'''
Return the configured motors.
'''
return self._motors
from colorama import init, Fore, Style
init()
from lib.logger import Level
from lib.motors import Motors
from lib.config_loader import ConfigLoader
# ..............................................................................
try:
print('brake :' + Fore.CYAN + Style.BRIGHT +
' INFO : braking...' + Style.RESET_ALL)
# read YAML configuration
_loader = ConfigLoader(Level.INFO)
filename = 'config.yaml'
_config = _loader.configure(filename)
_motors = Motors(_config, None, None, Level.INFO)
_motors.brake()
except KeyboardInterrupt:
print(Fore.RED + 'Ctrl-C caught in main: exiting...' + Style.RESET_ALL)
finally:
_motors.stop()
_motors.close()
print('brake :' + Fore.CYAN + Style.BRIGHT +
' INFO : complete.' + Style.RESET_ALL)
#EOF