class AbstractTask(ABC, FiniteStateMachine, threading.Thread):
'''
An abstract task class implemented as a Finite State Machine (FSM),
where transitions between states begin with an initial Start state,
proceeding through a series of controlled state transitions until
a Terminal state is reached.
The thread is started by a call to start() and terminated by close().
The _enabled variable is controlled by enable() and disable().
'''
sleep_delay = 1 # seconds
def __init__(self, task_name, priority, mutex, level=Level.INFO):
super().__init__(task_name)
super(FiniteStateMachine, self).__init__()
threading.Thread.__init__(self)
# initialise logger
self.task_name = task_name
self._log = Logger(task_name, level)
self._log.debug(task_name + '__init__() initialising...')
# initialise GPIO
self._priority = priority
self._mutex = mutex
self._active = True # set False only when closing task
self._enabled = False # enabled/disabled toggle
self._closing = False
self._number = -1
self._log.debug(task_name + '__init__() ready.')
def set_number(self, number):
self._number = number
def get_number(self):
return self._number
def get_priority(self):
return self._priority
def get_task_name(self):
return self.task_name
def __cmp__(self, other):
return cmp(self._priority, other._priority)
def __lt__(self, other):
return self._priority < other._priority
def is_active(self):
return self._active and self.is_alive()
def is_enabled(self):
return self._enabled
@abstractmethod
def run(self):
super(FiniteStateMachine, self).run()
self._log.debug('started ' + self.task_name + '.')
pass
@abstractmethod
def enable(self):
super().enable()
# super(FiniteStateMachine, self).enable()
self._log.debug('enabled ' + self.task_name + '.')
self._enabled = True
pass
@abstractmethod
def disable(self):
super().disable()
# super(FiniteStateMachine, self).disable()
self._enabled = False
self._log.debug('disabled ' + self.task_name + '.')
pass
@abstractmethod
def close(self):
super().close()
if not self._closing:
self._closing = True
self._active = False
self._enabled = False
self._log.critical('closing ' + self.task_name + '...')
_n = 0
while self.is_alive():
_n = _n + 1
self._log.info('joining thread of ' + self.task_name + '.')
self.join(timeout=2.0)
if self.task_name == 'os' and _n >= 3:
self._log.error(Fore.RED + Style.BRIGHT + 'waited too long: forced termination...')
sys.stderr = DevNull()
sys.exit()
self._log.info('waiting ({:d}) for {} to terminate...'.format(_n, self.task_name))
time.sleep(0.25)
self._log.critical('closed ' + self.task_name + '.')
# pass
else:
self._log.error('already closing ' + self.task_name + '.')
def making_host_decision(self, application, decision, release_node=False):
"""
Make decision on which host to run container
Args:
application(str)
decision(str)
Returns:
host(str)
"""
logger = Logger(filename="orchastrator",
logger_name="DecisionMaker making_host_decision")
swarm_manager = SwarmManagment()
app_per_node = "{}_per_node".format(application)
app_by_hosts = self.counting_app_by_host(application)
if release_node:
del (app_by_hosts[release_node])
host_number = len(app_by_hosts.keys())
if decision is 'up':
application_number = 0
for host in app_by_hosts.keys():
if app_by_hosts[host][application] == 0:
return host
else:
application_number += app_by_hosts[host][application]
average_app_number = application_number / host_number
# print("Average => {}".format(average_app_number))
# print("Appp => {}".format(parse_config('orchastrator.json')[app_per_node]))
logger.info("Aplication {} ||| Average => {}\tApp_per_node => {}". \
format(application, average_app_number, parse_config('orchastrator.json')[app_per_node]))
logger.clear_handler()
###logic for adding node to the swarm
if average_app_number >= parse_config(
'orchastrator.json')[app_per_node]:
if len(parse_config('orchastrator.json')
['available_servers']) != 0:
new_server = parse_config(
'orchastrator.json')['available_servers'][0]
swarm_manager.join_server_swarm(host_ip=parse_config(
'orchastrator.json')['available_servers'][0])
return new_server
else:
logger.critical("There are not any available servers should" \
"look at host stat to run on the lowest" \
"loaded host a container")
logger.clear_handler()
# print("There are not any available servers should \
# look at host stat to run on the lowest \
# loaded host a container")
###logic for adding node to the swarm
for host in app_by_hosts.keys():
if app_by_hosts[host][application] < average_app_number and \
app_by_hosts[host][application] < parse_config('orchastrator.json')[app_per_node]:
return host
for host in app_by_hosts.keys():
return host
elif decision is 'down':
application_number = 0
for host in app_by_hosts.keys():
application_number += app_by_hosts[host][application]
min_app = "{}_min".format(application)
# print("Min_app => {}\t app_num {}".format(parse_config('orchastrator.json')[min_app], application_number))
logger.warning("Application => {} ||| min_apps on platform=> {}\tcurrent app_num {}". \
format(application, parse_config('orchastrator.json')[min_app], application_number))
logger.clear_handler()
if application_number == parse_config(
'orchastrator.json')[min_app]:
return None
average_app_number = application_number / host_number
for host in app_by_hosts.keys():
if app_by_hosts[host][application] > average_app_number and \
app_by_hosts[host][application] < parse_config('orchastrator.json')[app_per_node]:
return host
for host in app_by_hosts.keys():
return host
class Motor():
'''
Example of a motor sequence script that uses the motor
encoders to determine the distance traveled.
This uses the ros:motors: section of the configuration.
'''
def __init__(self, config, tb, pi, orientation, level):
global TB
super(Motor, self).__init__()
if config is None:
raise ValueError('null configuration argument.')
if tb is None:
raise ValueError('null thunderborg argument.')
self._tb = tb
TB = tb
if pi is None:
raise ValueError('null pi argument.')
self._pi = pi
# configuration ....................................
# get motors configuration section
cfg = config['ros'].get('motors')
# in case you wire something up backwards (we need this prior to the logger)
self._reverse_motor_orientation = cfg.get('reverse_motor_orientation')
# establish orientation
if self._reverse_motor_orientation:
self._orientation = Orientation.STBD if orientation is Orientation.PORT else Orientation.PORT
else:
self._orientation = orientation
# NOW we can create the logger
self._log = Logger('motor:{}'.format(orientation.label), level)
self._log.info('initialising {} motor...'.format(orientation))
self._log.debug('_reverse_motor_orientation: {}'.format(
self._reverse_motor_orientation))
self._reverse_encoder_orientation = cfg.get(
'reverse_encoder_orientation')
self._log.debug('_reverse_encoder_orientation: {}'.format(
self._reverse_encoder_orientation))
# GPIO pins configured for A1, B1, A2 and B2
self._rotary_encoder_a1_port = cfg.get(
'rotary_encoder_a1_port') # default: 22
self._log.debug('rotary_encoder_a1_port: {:d}'.format(
self._rotary_encoder_a1_port))
self._rotary_encoder_b1_port = cfg.get(
'rotary_encoder_b1_port') # default: 17
self._log.debug('rotary_encoder_b1_port: {:d}'.format(
self._rotary_encoder_b1_port))
self._rotary_encoder_a2_stbd = cfg.get(
'rotary_encoder_a2_stbd') # default: 27
self._log.debug('rotary_encoder_a2_stbd: {:d}'.format(
self._rotary_encoder_a2_stbd))
self._rotary_encoder_b2_stbd = cfg.get(
'rotary_encoder_b2_stbd') # default: 18
self._log.debug('rotary_encoder_b2_stbd: {:d}'.format(
self._rotary_encoder_b2_stbd))
# how many pulses per encoder measurement?
self._sample_rate = cfg.get('sample_rate') # default: 10
self._log.debug('sample_rate: {:d}'.format(self._sample_rate))
# convert raw velocity to approximate a percentage
self._velocity_fudge_factor = cfg.get(
'velocity_fudge_factor') # default: 14.0
self._log.debug('velocity fudge factor: {:>5.2f}'.format(
self._velocity_fudge_factor))
# limit set on power sent to motors
self._max_power_limit = cfg.get('max_power_limit') # default: 1.2
self._log.debug('maximum power limit: {:>5.2f}'.format(
self._max_power_limit))
# acceleration loop delay
self._accel_loop_delay_sec = cfg.get(
'accel_loop_delay_sec') # default: 0.10
self._log.debug('acceleration loop delay: {:>5.2f} sec'.format(
self._accel_loop_delay_sec))
# end configuration ................................
self._motor_power_limit = 0.99 # power limit to motor
self._steps = 0 # step counter
self._steps_begin = 0 # step count at beginning of velocity measurement
self._velocity = 0.0 # current velocity
self._max_velocity = 0.0 # capture maximum velocity attained
self._max_power = 0.0 # capture maximum power applied
self._max_driving_power = 0.0 # capture maximum adjusted power applied
self._interrupt = False # used to interrupt loops
self._stepcount_timestamp = time.time(
) # timestamp at beginning of velocity measurement
self._start_timestamp = time.time(
) # timestamp at beginning of velocity measurement
# configure encoder ................................
self._log.info('configuring rotary encoders...')
if self._reverse_encoder_orientation:
if orientation is Orientation.PORT:
self.configure_encoder(Orientation.STBD)
else:
self.configure_encoder(Orientation.PORT)
else:
self.configure_encoder(self._orientation)
self._log.info('ready.')
# ..............................................................................
@property
def velocity(self):
return self._velocity
# ..............................................................................
def get_steps(self):
return self._steps
# ..............................................................................
def reset_steps(self):
self._steps = 0
# ..............................................................................
def set_max_power_ratio(self, max_power_ratio):
self._max_power_ratio = max_power_ratio
# ..............................................................................
def get_max_power_ratio(self):
return self._max_power_ratio
# ..............................................................................
def configure_encoder(self, orientation):
if self._orientation is Orientation.PORT:
ROTARY_ENCODER_A = self._rotary_encoder_a1_port
ROTARY_ENCODER_B = self._rotary_encoder_b1_port
elif self._orientation is Orientation.STBD:
ROTARY_ENCODER_A = self._rotary_encoder_a2_stbd
ROTARY_ENCODER_B = self._rotary_encoder_b2_stbd
else:
raise ValueError("unrecognised value for orientation.")
self._decoder = Decoder(self._pi, ROTARY_ENCODER_A, ROTARY_ENCODER_B,
self.callback_step_count)
self._log.info('configured {} rotary encoder on pin {} and {}.'.format(
orientation.name, ROTARY_ENCODER_A, ROTARY_ENCODER_B))
# ..............................................................................
def callback_step_count(self, pulse):
'''
This callback is used to capture encoder steps.
'''
if not self._reverse_encoder_orientation:
if self._orientation is Orientation.PORT:
self._steps = self._steps + pulse
else:
self._steps = self._steps - pulse
else:
if self._orientation is Orientation.PORT:
self._steps = self._steps - pulse
else:
self._steps = self._steps + pulse
if self._steps % self._sample_rate == 0:
if self._steps_begin != 0:
self._velocity = (
(self._steps - self._steps_begin) /
(time.time() - self._stepcount_timestamp) /
self._velocity_fudge_factor) # steps / duration
self._max_velocity = max(self._velocity, self._max_velocity)
self._stepcount_timestamp = time.time()
self._stepcount_timestamp = time.time()
self._steps_begin = self._steps
# self._log.info(Fore.BLACK + '{}: {:+d} steps'.format(self._orientation.label, self._steps))
# ..............................................................................
def cruise():
'''
Cruise at the current speed.
'''
pass # TODO
# ..........................................................................
def enable(self):
self._log.info('enabled.')
pass
# ..........................................................................
def disable(self):
self._log.info('disabled.')
pass
# ..........................................................................
def close(self):
self._log.info(
'max velocity: {:>5.2f}; max power: {:>5.2f}; max adjusted power: {:>5.2f}.'
.format(self._max_velocity, self._max_power,
self._max_driving_power))
self._log.info('closed.')
# ..........................................................................
def interrupt(self):
'''
Interrupt any loops by setting the _interrupt flag.
'''
self._interrupt = True
# ..........................................................................
def reset_interrupt(self):
'''
Reset the value of the _interrupt flag to False.
'''
self._interrupt = False
# ..........................................................................
def is_interrupted(self):
'''
Return the value of the _interrupt flag.
'''
return self._interrupt
# ..........................................................................
@staticmethod
def cancel():
'''
Stop both motors immediately. This can be called from either motor.
'''
try:
TB
except NameError:
TB = None
if TB:
TB.SetMotor1(0.0)
TB.SetMotor2(0.0)
else:
print('motor :' + Fore.YELLOW +
' WARN : cannot cancel motors: no thunderborg available.' +
Style.RESET_ALL)
# Motor Functions ............................................................
# ..........................................................................
def stop(self):
'''
Stops the motor immediately.
'''
self._log.info('stop.')
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
pass
# ..........................................................................
def halt(self):
'''
Quickly (but not immediately) stops.
'''
self._log.info('halting...')
# set slew slow, then decelerate to zero
self.accelerate(0.0, SlewRate.FAST, -1)
self._log.debug('halted.')
# ..........................................................................
def brake(self):
'''
Slowly coasts to a stop.
'''
self._log.info('braking...')
# set slew slow, then decelerate to zero
self.accelerate(0.0, SlewRate.SLOWER, -1)
self._log.debug('braked.')
# ..........................................................................
def ahead(self, speed):
'''
Slews the motor to move ahead at speed.
This is an alias to accelerate(speed).
'''
self._log.info('ahead to speed of {}...'.format(speed))
self.accelerate(speed, SlewRate.NORMAL, -1)
self._log.debug('ahead complete.')
# ..........................................................................
def stepAhead(self, speed, steps):
'''
Moves forwards specified number of steps, then stops.
'''
# self._log.info('step ahead {} steps to speed of {}...'.format(steps,speed))
self.accelerate(speed, SlewRate.NORMAL, steps)
# self._log.debug('step ahead complete.')
pass
# ..........................................................................
def astern(self, speed):
'''
Slews the motor to move astern at speed.
This is an alias to accelerate(-1 * speed).
'''
self._log.info('astern at speed of {}...'.format(speed))
self.accelerate(-1.0 * speed, SlewRate.NORMAL, -1)
self._log.debug('astern complete.')
# ..........................................................................
def stepAstern(self, speed, steps):
'''
Moves backwards specified number of steps, then stops.
'''
self._log.info('step astern {} steps to speed of {}...'.format(
steps, speed))
self.accelerate(speed, SlewRate.NORMAL, steps)
self._log.debug('step astern complete.')
pass
# ..........................................................................
def is_in_motion(self):
'''
Returns true if the motor is moving.
'''
return self.get_current_power_level() > 0.0
# ..........................................................................
def accelerate_to_velocity(self, velocity, slew_rate, steps):
'''
Slews the motor to the requested velocity.
If steps is greater than zero it provides a step limit.
'''
if steps > 0:
_step_limit = self._steps + steps
self._log.critical('>>>>>> {} steps, limit: {}.'.format(
self._steps, _step_limit))
else:
_step_limit = -1
if self._velocity == velocity: # if current velocity equals requested, no need to accelerate
self._log.info(
'NO CHANGE: ALREADY AT velocity {:>5.2f}/{:>5.2f}.'.format(
self._velocity, velocity))
return
# accelerate to target velocity...
self._accelerate_to_velocity(velocity, slew_rate, _step_limit)
self._log.info(Fore.YELLOW +
'REACHED TARGET VELOCITY: {:>5.2f}, now maintaining...'.
format(velocity))
self._log.info(Fore.BLUE + Style.BRIGHT +
'accelerated to velocity {:>5.2f} at power: {:>5.2f}. '.
format(velocity, self.get_current_power_level()))
# ..........................................................................
def _accelerate_to_velocity(self, velocity, slew_rate, step_limit):
_current_power_level = self.get_current_power_level() * (
1.0 / self._max_power_ratio)
self._log.info(Fore.BLUE + Style.BRIGHT + '_accelerate_to_velocity {:>5.2f} @ slew rate: {:>5.2f}; current power: {:>5.2f}; step limit: {:+d}'.format(\
velocity, slew_rate.ratio, _current_power_level, step_limit))
if self._velocity < velocity: # if current velocity is less than requested, speed up
# note that steps count down when going in reverse
self._log.info(
'NEED TO SPEED UP TO velocity {:>5.2f}/{:>5.2f}.'.format(
self._velocity, velocity))
while self._velocity < velocity and (step_limit == -1
or self._steps <= step_limit):
_current_power_level += slew_rate.ratio
# FIXME as we get closer to our goal we tend to bounce
driving_power_level = float(_current_power_level *
self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._orientation is Orientation.PORT:
self._log.info( Fore.RED + 'INCREASE: ' + Fore.CYAN + 'velocity {:>5.2f} < {:>5.2f};\t{:+d} of {:+d} steps;\tcurrent power: {:>5.2f}.'.format(\
self._velocity, velocity, self._steps, step_limit, _current_power_level))
else:
self._log.info( Fore.GREEN + 'INCREASE: ' + Fore.CYAN + 'velocity {:>5.2f} < {:>5.2f};\t{:+d} of {:+d} steps;\tcurrent power: {:>5.2f}.'.format(\
self._velocity, velocity, self._steps, step_limit, _current_power_level))
if self._interrupt:
break
time.sleep(0.1)
elif self._velocity > velocity: # if current velocity is greater than requested, slow down
self._log.info(
'NEED TO SLOW DOWN TO velocity {:>5.2f}/{:>5.2f}.'.format(
self._velocity, velocity))
while self._velocity > velocity and (step_limit == -1
or self._steps <= step_limit):
_current_power_level -= slew_rate.ratio
# FIXME as we get closer to our goal we tend to bounce
driving_power_level = float(_current_power_level *
self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._orientation is Orientation.PORT:
self._log.info( Fore.RED + 'DECREASE: ' + Fore.BLUE + 'WHILE velocity {:>5.2f} < {:>5.2f}: steps: {} of {}; current power level: {:>5.2f}.'.format(\
self._velocity, velocity, self._steps, step_limit, _current_power_level))
else:
self._log.info( Fore.GREEN + 'DECREASE: ' + Fore.BLUE + 'WHILE velocity {:>5.2f} < {:>5.2f}: steps: {} of {}; current power level: {:>5.2f}.'.format(\
self._velocity, velocity, self._steps, step_limit, _current_power_level))
if self._interrupt:
break
time.sleep(0.1)
# self._log.info(Fore.CYAN + 'WHILE velocity {:>5.2f} < {:>5.2f}: current power level: {:>5.2f}.'.format(self._velocity,velocity, _current_power_level))
# ..........................................................................
def ahead_for_steps(self, speed, steps):
'''
A test method that runs the motor ahead at the provided Speed,
returning once the number of steps have been reached. As this
provides for no slewing, in order to not put too much stress on
the motor, do not call with a high speed.
'''
_BRAKING = False
_current_steps = self._steps # current steps
_braking_range = 500 if steps > 1000 else 100
_speed = speed / 100.0
_power = float(_speed * self._max_power_ratio)
if _BRAKING:
self._log.info(
Fore.YELLOW +
'ahead at speed {:>5.2f} using power {:>5.2f} for steps: {:d} with braking range of {:d}.'
.format(_speed, _power, steps, _braking_range))
else:
self._log.info(
Fore.YELLOW +
'ahead at speed {:>5.2f} using power {:>5.2f} for steps: {:d} with no braking range.'
.format(_speed, _power, steps))
self.set_motor_power(_power)
if speed >= 0:
if _BRAKING:
_step_limit = _current_steps + steps - _braking_range
while self._steps < _step_limit:
self._log.info('{:d}/{:d} steps.'.format(
self._steps, _step_limit))
if self._interrupt:
break
time.sleep(0.01)
# now brake to stop ........................
_braking_step_limit = _current_steps + steps
while self._steps < _braking_step_limit:
_speed = max(0.20, _speed - 0.001)
_power = float(_speed * self._max_power_ratio)
self.set_motor_power(_power)
self._log.info(
Fore.RED +
'{:d}/{:d} steps at speed/power: {:>5.2f}/{:>5.2f}.'.
format(self._steps, _braking_step_limit, _speed,
_power))
if self._interrupt:
break
time.sleep(0.01)
else:
_step_limit = _current_steps + steps
while self._steps < _step_limit:
_near_end = self._steps > _step_limit - 494
if _near_end:
self._log.debug(Fore.CYAN + Style.BRIGHT +
'{:d}/{:d} steps.'.format(
self._steps, _step_limit))
else:
self._log.debug('{:d}/{:d} steps.'.format(
self._steps, _step_limit))
if self._interrupt:
break
time.sleep(0.01)
else:
_step_limit = _current_steps - steps + _braking_range
while self._steps > _step_limit:
self._log.info('{:d}/{:d} steps.'.format(
self._steps, _step_limit))
if self._interrupt:
break
time.sleep(0.01)
# now brake to stop ........................
_braking_step_limit = _current_steps - steps
while self._steps > _braking_step_limit:
_speed = min(0.25, _speed + 0.002)
_power = float(_speed * self._max_power_ratio)
self.set_motor_power(_power)
self._log.info(
Fore.RED +
'{:d}/{:d} steps at speed/power: {:>5.2f}/{:>5.2f}.'.
format(self._steps, _braking_step_limit, _speed, _power))
if self._interrupt:
break
time.sleep(0.01)
self.stop()
self._interrupt = False
# ..........................................................................
def step_to(self, steps):
'''
Advances the motor to the requested step count, attempting to stop on that mark.
This exits if we're not moving.
'''
if steps <= 0:
self._log.warning(
'can\'t advance to step: argument less than zero.')
return
elif not self.is_in_motion():
self._log.warning('can\'t advance to step: we\re not moving.')
return
elif self._velocity == 0.0:
self._log.warning('can\'t advance to step: we have no velocity.')
return
elif steps < self._steps:
self._log.warning(
'can\'t advance to step: we\re already past the point.')
return
orient = Fore.RED if self._orientation is Orientation.PORT else Fore.GREEN
# maintain velocity until within braking range
braking_range = steps - 500
while self._steps <= braking_range:
self._log.info(orient +
'step {:+d} of {:+d}.'.format(self._steps, steps))
time.sleep(0.1)
self._log.info(orient + 'BRAKING from step {:+d}.'.format(self._steps))
# now crawl to the end...
_speed = Speed.DEAD_SLOW.value
_slew_rate = SlewRate.NORMAL
_desired_div_100 = float(_speed / 100.0)
_current_power_level = self.get_current_power_level()
self._log.info(orient +
'accelerating from {:>5.2f} to {:>5.2f}...'.format(
_current_power_level, _desired_div_100))
if _current_power_level == _desired_div_100: # no change
self._log.warning(
orient +
'already at acceleration power of {:>5.2f}, exiting.'.format(
_current_power_level))
return
elif _current_power_level < _desired_div_100: # moving ahead
_slew_rate_ratio = _slew_rate.ratio
overstep = 0.001
else: # moving astern
_slew_rate_ratio = -1.0 * _slew_rate.ratio
overstep = -0.001
self._log.warning(
orient + Style.BRIGHT +
'LOOP from {:>5.2f} to limit: {:>5.2f} with slew: {:>5.2f}'.format(
_current_power_level, (_desired_div_100 +
overstep), _slew_rate_ratio))
driving_power_level = 0.0
for step_power in numpy.arange(_current_power_level,
(_desired_div_100 + overstep),
_slew_rate_ratio):
driving_power_level = float(step_power * self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._interrupt:
break
time.sleep(self._accel_loop_delay_sec)
self._log.info(orient +
'FINISHED BRAKING from step {:+d}.'.format(self._steps))
# braking_power_level = _current_power_level * 0.5
# if self._orientation is Orientation.PORT:
# self._tb.SetMotor1(braking_power_level)
# else:
# self._tb.SetMotor2(braking_power_level)
# while self._steps <= steps:
# self._log.info( orient + 'step {:+d} of {:+d}. BRAKING...'.format(self._steps, steps))
# time.sleep(0.1)
# now halt entirely
self.halt()
self._log.info('BREAK FROM STEP TO loop at step {:+d}.'.format(
self._steps))
# ..........................................................................
def set_motor_power(self, power_level):
'''
Sets the power level to a number between 0.0 and 1.0, with the
actual limits set both by the _max_driving_power limit and by
the _max_power_ratio, which alters the value to match the
power/motor voltage ratio.
'''
# safety checks ..........................
if power_level > self._motor_power_limit:
self._log.error(Style.BRIGHT +
'motor power too high: {:>5.2f}; limit: {:>5.2f}'.
format(power_level, self._motor_power_limit))
return
elif power_level < (-1.0 * self._motor_power_limit):
self._log.error(
Style.BRIGHT +
'motor power too low: {:>5.2f}; limit: {:>5.2f}'.format(
power_level, (-1.0 * self._motor_power_limit)))
return
_current_power = self.get_current_power_level()
# _current_actual_power = _current_power * ( 1.0 / self._max_power_ratio )
if abs(_current_power -
power_level) > 0.3 and _current_power > 0.0 and power_level < 0:
self._log.error(
'cannot perform positive-negative power jump: {:>5.2f} to {:>5.2f}.'
.format(_current_power, power_level))
return
elif abs(_current_power - power_level
) > 0.3 and _current_power < 0.0 and power_level > 0:
self._log.error(
'cannot perform negative-positive power jump: {:>5.2f} to {:>5.2f}.'
.format(_current_power, power_level))
return
# okay, let's go .........................
_driving_power = float(power_level * self._max_power_ratio)
self._max_power = max(power_level, self._max_power)
self._max_driving_power = max(abs(_driving_power),
self._max_driving_power)
self._log.debug(Fore.MAGENTA + Style.BRIGHT + 'power argument: {:>5.2f}'.format(power_level) + Style.NORMAL \
+ '\tcurrent power: {:>5.2f}; driving power: {:>5.2f}.'.format(_current_power, _driving_power))
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(_driving_power)
else:
self._tb.SetMotor2(_driving_power)
# ..........................................................................
def accelerate_to_zero(self, slew_rate):
'''
Slews the motor to a stop.
'''
_current_power_level = self.get_current_power_level() * (
1.0 / self._max_power_ratio)
if self._velocity == 0.0: # if current velocity equals zero, do nothing
self._log.info('NO CHANGE: ALREADY STOPPED.')
return
elif self._velocity < 0.0: # if current velocity is in reverse
self._log.info('NEED TO SPEED UP TO ZERO.')
while self._velocity < 0.0:
driving_power_level = float(_current_power_level *
self._max_power_ratio)
set_motor_power(driving_power_level)
if self._interrupt:
break
_current_power_level += slew_rate.ratio
time.sleep(0.1)
self._log.info(
Fore.CYAN +
'WHILE velocity {:>5.2f} < zero: current power level: {:>5.2f}.'
.format(self._velocity, _current_power_level))
elif self._velocity > 0.0: # if current velocity is greater than zero, slow to a stop
self._log.info('NEED TO SLOW DOWN TO zero')
while self._velocity > 0.0:
driving_power_level = float(_current_power_level *
self._max_power_ratio)
set_motor_power(driving_power_level)
if self._interrupt:
break
_current_power_level -= slew_rate.ratio
time.sleep(0.1)
self._log.info(
Fore.CYAN +
'WHILE velocity {:>5.2f} > zero: current power level: {:>5.2f}.'
.format(self._velocity, _current_power_level))
time.sleep(2)
self._log.info(Fore.BLUE +
'BREAK LOOP (accelerate to zero) at power: {:>5.2f}. '.
format(self.get_current_power_level()))
time.sleep(2)
# be sure we're entirely powered off
if self.get_current_power_level() > 0.0:
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
time.sleep(2)
# ..........................................................................
def accelerate(self, speed, slew_rate, steps):
'''
Slews the motor to the designated speed. -100 <= 0 <= speed <= 100.
This takes into account the maximum power to be supplied to the
motor based on the battery and motor voltages.
If steps > 0 then run until the number of steps has been reached.
'''
self._interrupt = False
_current_power_level = self.get_current_power_level()
if _current_power_level is None:
raise RuntimeError(
'cannot continue: unable to read current power from motor.')
self._log.info(
'current power: {:>5.2f} max power ratio: {:>5.2f}...'.format(
_current_power_level, self._max_power_ratio))
_current_power_level = _current_power_level * (1.0 /
self._max_power_ratio)
# accelerate to desired speed
_desired_div_100 = float(speed / 100)
self._log.info('accelerating from {:>5.2f} to {:>5.2f}...'.format(
_current_power_level, _desired_div_100))
if _current_power_level == _desired_div_100: # no change
self._log.warning(
'already at acceleration power of {:>5.2f}, exiting.'.format(
_current_power_level))
return
elif _current_power_level < _desired_div_100: # moving ahead
_slew_rate_ratio = slew_rate.ratio
overstep = 0.001
else: # moving astern
_slew_rate_ratio = -1.0 * slew_rate.ratio
overstep = -0.001
# self._log.warning(Style.BRIGHT + 'LOOP from {:>5.2f} to limit: {:>5.2f} with slew: {:>5.2f}'.format(_current_power_level, (_desired_div_100 + overstep), _slew_rate_ratio))
driving_power_level = 0.0
for step_power in numpy.arange(_current_power_level,
(_desired_div_100 + overstep),
_slew_rate_ratio):
driving_power_level = float(step_power * self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._interrupt:
break
time.sleep(self._accel_loop_delay_sec)
# be sure we're powered off
if speed == 0.0 and abs(driving_power_level) > 0.00001:
self._log.warning(
'non-zero power level: {:7.5f}v; stopping completely...'.
format(driving_power_level))
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
self._log.debug('accelerate complete.')
return
# ..........................................................................
def accelerate_while(self, speed, direction, slew_rate, f_is_enabled):
'''
Slews the motor to the designated speed. -100 <= 0 <= speed <= 100.
This takes into account the maximum power to be supplied to the
motor based on the battery and motor voltages.
This runs while the provided function returns True, then exits.
'''
self._interrupt = False
_current_power_level = self.get_current_power_level()
if _current_power_level is None:
raise RuntimeError(
'cannot continue: unable to read current power from motor.')
self._log.info(
'current power: {:>5.2f} max power ratio: {:>5.2f}...'.format(
_current_power_level, self._max_power_ratio))
_current_power_level = _current_power_level * (1.0 /
self._max_power_ratio)
# accelerate to desired speed
_desired_div_100 = float(speed / 100)
if direction is Direction.REVERSE:
_desired_div_100 = -1 * _desired_div_100
self._log.info('accelerating from {:>5.2f} to {:>5.2f}...'.format(
_current_power_level, _desired_div_100))
if _current_power_level == _desired_div_100: # no change
self._log.warning(
'already at acceleration power of {:>5.2f}, exiting.'.format(
_current_power_level))
return
elif _current_power_level < _desired_div_100: # moving ahead
_slew_rate_ratio = slew_rate.ratio
overstep = 0.001
else: # moving astern
_slew_rate_ratio = -1.0 * slew_rate.ratio
overstep = -0.001
# self._log.warning(Style.BRIGHT + 'LOOP from {:>5.2f} to limit: {:>5.2f} with slew: {:>5.2f}'.format(_current_power_level, (_desired_div_100 + overstep), _slew_rate_ratio))
driving_power_level = 0.0
for step_power in numpy.arange(_current_power_level,
(_desired_div_100 + overstep),
_slew_rate_ratio):
driving_power_level = float(step_power * self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._interrupt or not f_is_enabled:
break
time.sleep(self._accel_loop_delay_sec)
# be sure we're powered off
if speed == 0.0 and abs(driving_power_level) > 0.00001:
self._log.warning(
'non-zero power level: {:7.5f}v; stopping completely...'.
format(driving_power_level))
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
self._log.debug('accelerate complete.')
return
# ..........................................................................
def get_orientation(self):
'''
Returns the orientation of this motor.
'''
return self._orientation
# ..........................................................................
def is_stopped(self):
'''
Returns true if the motor is entirely stopped.
'''
return (self.get_current_power_level() == 0.0)
# ..........................................................................
'''
These two methods store the last set value of each motor.
This is actually set from z_motor.
'''
def set_last_set_power(self, orientation, value):
if orientation is Orientation.PORT:
# self._last_set_power[0] = value
# self._log.debug('set_last_set_power({},{:6.1f})'.format(orientation, self._last_set_power[0]))
pass
else:
# self._last_set_power[1] = value
# self._log.debug('set_last_set_power({},{:6.1f})'.format(orientation, self._last_set_power[1]))
pass
# ................................
def get_current_power_level(self):
'''
Makes a best attempt at getting the power level value from the motors.
'''
value = None
count = 0
if self._orientation is Orientation.PORT:
while value == None and count < 20:
count += 1
value = self._tb.GetMotor1()
time.sleep(0.005)
else:
while value == None and count < 20:
count += 1
value = self._tb.GetMotor2()
time.sleep(0.005)
if value == None:
return 0.0
else:
return value
from lib.logger import Logger
logger = Logger()
logger.error("BLQ BLA")
logger.info("BLQ asdfsdf")
logger.critical("BLQ sdfasdfasd")
# import logging
# logger = logging.getLogger()
# logger.setLevel(logging.INFO)
# # create the logging file handler
# fh = logging.FileHandler("new_snake.log")
# formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
# fh.setFormatter(formatter)
# # add handler to logger object
# logger.addHandler(fh)
# logger.info("Program started")
class CatScan():
'''
Uses an infrared PIR sensor to scan for cats. This involves raising a
servo arm holding the sensor.
'''
def __init__(self, config, queue, level):
self._log = Logger('cat-scan', Level.INFO)
self._config = config
self._queue = queue
if self._config:
self._log.info('configuration provided.')
_config = self._config['ros'].get('cat_scan')
self._active_angle = _config.get('active_angle')
self._inactive_angle = _config.get('inactive_angle')
_pir_pin = _config.get('pir_pin')
_servo_number = _config.get('servo_number')
else:
self._log.warning('no configuration provided.')
self._active_angle = -90.0
self._inactive_angle = 90.0
_pir_pin = 15
_servo_number = 3
_threshold_value = 0.1 # the value above which the device will be considered “on”
self._log.info(
'cat scan active angle: {:>5.2f}°; inactive angle: {:>5.2f}°'.
format(self._active_angle, self._inactive_angle))
self._servo = Servo(self._config, _servo_number, level)
# set up pin where PIR is connected
self._sensor = MotionSensor(_pir_pin,
threshold=_threshold_value,
pull_up=False)
self._sensor.when_motion = self._activated
self._sensor.when_no_motion = None #self._deactivated
self._scan_enabled = False
self._disabling = False
self._enabled = False
self._closed = False
# arm behaviour
self._arm_movement_degree_step = 5.0
self._arm_up_delay = 0.09
self._arm_down_delay = 0.04
self._rgbmatrix = RgbMatrix(Level.INFO)
self._rgbmatrix.set_display_type(DisplayType.SCAN)
self._log.info('ready.')
# ..........................................................................
def _warning_display(self):
self._rgbmatrix.disable()
self._rgbmatrix.set_display_type(DisplayType.RANDOM)
self._rgbmatrix.enable()
# ..........................................................................
def _activated(self):
'''
The default function called when the sensor is activated.
'''
if self._enabled and self._scan_enabled:
self._log.info('cat sensed.')
self._log.info(Fore.RED + Style.BRIGHT + 'detected CAT!')
self._queue.add(Message(Event.CAT_SCAN))
self.set_mode(False)
self._warning_display()
else:
self._log.info('[DISABLED] activated catscan.')
# ..........................................................................
def _deactivated(self):
'''
The default function called when the sensor is deactivated.
'''
if self._enabled:
self._log.info('deactivated catscan.')
else:
self._log.debug('[DISABLED] deactivated catscan.')
# ..........................................................................
def is_waiting(self):
'''
Returns true if the scan is in progress.
'''
return self._scan_enabled
# ..........................................................................
def set_mode(self, active):
'''
Sets the mode to active or inactive. This raises or lowers the servo arm.
'''
if not self._enabled:
self._log.warning('cannot scan: disabled.')
return
_current_angle = self._servo.get_position(-999.0)
self._log.critical('current angle: {:>4.1f}°'.format(_current_angle))
if active:
if _current_angle != -999.0 and _current_angle is not self._active_angle:
with _mutex:
# for degrees in numpy.arange(self._inactive_angle, self._active_angle + 0.01, -1.0 * self._arm_movement_degree_step):
for degrees in numpy.arange(
_current_angle, self._active_angle + 0.01,
-1.0 * self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_up_delay)
self._log.info('position set to active.')
else:
self._log.warning('position already set to active.')
# wait until it settles
self._log.info('waiting for motion detector to settle...')
time.sleep(1.0)
while self._sensor.motion_detected:
self._log.debug('still waiting...')
time.sleep(0.1)
# time.sleep(5.0)
self._scan_enabled = True
self._rgbmatrix.enable()
else:
self._scan_enabled = False
self._rgbmatrix.disable()
if _current_angle != -999.0 and _current_angle is not self._inactive_angle:
with _mutex:
for degrees in numpy.arange(
self._active_angle, self._inactive_angle + 0.1,
self._arm_movement_degree_step):
self._log.debug(
'position set to: {:>4.1f}°'.format(degrees))
self._servo.set_position(degrees)
time.sleep(self._arm_down_delay)
self._log.info('position set to inactive.')
else:
self._log.warning('position already set to inactive.')
# ..........................................................................
def enable(self):
self._log.debug('enabling...')
if self._closed:
self._log.warning('cannot enable: closed.')
return
# self._tof.enable()
self._enabled = True
self._log.debug('enabled.')
# ..........................................................................
def disable(self):
if self._disabling:
self._log.warning('already disabling.')
else:
self._disabling = True
self._enabled = False
self._rgbmatrix.disable()
self._log.debug('disabling...')
self.set_mode(False)
self._servo.disable()
self._servo.close()
self._disabling = False
self._log.debug('disabled.')
# ..........................................................................
def close(self):
self.disable()
self._closed = True
class Arbitrator(threading.Thread):
'''
Arbitrates a stream of events from a MessageQueue according to
priority, returning to a Controller the highest priority of them.
The Controller API is:
.get_current_message() returns the last message received
.act(_current_message, _action_complete_callback)
act upon the current message, with
a callback called upon completion
'''
def __init__(self, config, queue, controller, level):
super().__init__()
threading.Thread.__init__(self)
self._log = Logger('arbitrator', level)
if config is None:
raise ValueError('no configuration provided.')
self._config = config['ros'].get('arbitrator')
self._idle_loop_count = 0
self._loop_delay_sec = self._config.get('loop_delay_sec')
self._ballistic_loop_delay_sec = self._config.get(
'ballistic_loop_delay_sec')
self._queue = queue
self._controller = controller
self._tasks = []
self._is_enabled = True
self._closing = False
self._closed = False
self._suppressed = False
self._counter = itertools.count()
self._log.debug('ready.')
# ..........................................................................
def set_suppressed(self, suppressed):
self._suppressed = suppressed
self._log.info('suppressed: {}'.format(suppressed))
# ..........................................................................
def enable(self):
self._log.info('enabled.')
self._is_enabled = True
# ..........................................................................
def disable(self):
self._log.info('disabled.')
self._is_enabled = False
# ..........................................................................
def run(self):
self._log.info('arbitrating tasks...')
while self._is_enabled:
_start_time = dt.datetime.now()
self._loop_count = next(self._counter)
if self._suppressed:
# if suppressed just clear the queue so events don't build up
self._queue.clear()
else:
# there are 604800 seconds in a week, 6 decimal places should do...
self._log.debug(
'loop {:06d} begins with queue of {} elements.'.format(
self._loop_count, self._queue.size()))
next_messages = self._queue.next_group(5)
if len(next_messages) == 0:
self._log.debug('message queue was empty.'.format(
len(next_messages)))
elif len(next_messages) == 1:
self._log.debug(
'obtained one message from queue...'.format(
len(next_messages)))
else:
self._log.debug(
'obtained {} messages from queue...'.format(
len(next_messages)))
first_message = True
for i in range(len(next_messages)):
next_message = next_messages[i]
if first_message:
self._idle_loop_count = 0 # reset
self.accept_highest_priority_message(next_message)
else:
self._log.debug('{}: message #{:07d};\tpriority #{}: {}.'.format(i, \
next_message.get_number(), next_message.get_priority(), next_message.get_description()))
first_message = False
# we don't care about the messages in the queue that weren't high enough priority
self._queue.clear()
_current_message = self._controller.get_current_message()
if _current_message is not None:
if _current_message.get_event() == Event.STANDBY:
self._log.debug(
'{:06d} : current event: {}; queue: {} elements.'.
format(self._loop_count,
_current_message.get_event().description,
self._queue.size()))
if (self._loop_count % 10) == 0:
self._log.info('{:06d} : standing by...'.format(
self._loop_count))
else:
self._log.info(
'{:06d} : event: {}; queue: {} elements.'.format(
self._loop_count,
_current_message.get_event().description,
self._queue.size()))
else: # no messages: we're idle.
self._idle_loop_count += 1
if self._idle_loop_count <= 500:
if (self._loop_count % 50) == 0:
self._log.info('{:06d} : idle.'.format(
self._loop_count))
else: # after being idle for a long time, dim the message
if (self._loop_count % 500) == 0:
self._log.info('{:06d} : idle...'.format(
self._loop_count))
time.sleep(self._loop_delay_sec)
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.info('elapsed: {}ms'.format(_elapsed_ms))
self._log.info('loop end.')
# ..........................................................................
def interrupt(self, message):
'''
If the current Action was not ballistic and the new one is a different Action,
interrupt the old Action. We can't interrupt a ballistic Action.
'''
_current_message = self._controller.get_current_message()
_current_event = _current_message.get_event(
) if _current_message is not None else None
# [on_true] if [expression] else [on_false]
_new_event = message.get_event()
if _current_event == _new_event:
self._log.critical(
'NO CHANGE in event {} (ballistic? {})...'.format(
_new_event.name, _new_event.is_ballistic))
elif _current_event.is_ballistic:
self._log.critical(
'NOT INTERRUPTING current ballistic event {} (ballistic? {})...'
.format(_current_event.name, _current_event.is_ballistic))
else:
self._log.critical(
'interrupting with new event {} (ballistic? {})...'.format(
_new_event.name, _new_event.is_ballistic))
message.interrupt()
self._motors.interrupt()
# ...
# ..........................................................................
def accept_highest_priority_message(self, message):
'''
Accepts a new message. If the contained Action is the same as the previous Action,
no change is warranted and we return immediately.
If the current Action is not ballistic and the new Action is different, interrupt
the current Action. We don't interrupt a ballistic Action.
'''
if message is None:
raise TypeError
_number = message.get_number()
_description = message.get_description()
self._log.info(
'accept highest priority message {}; description: {}'.format(
_number, _description))
_current_message = self._controller.get_current_message()
if _current_message is None:
self._log.info('there is no existing message.')
else:
self._log.info('existing message is not None.')
if _current_message == message:
self._log.warning('NO CHANGE: message #{:07d};\tevent: {}; priority #{}; value: {}.'.format(\
message.get_number(), message.get_event().description, message.get_priority(), message.get_value()))
return
if not _current_message.get_event().is_ballistic:
self._log.debug('existing action is ballistic.')
self.interrupt(_current_message)
_current_message = message
self._log.info('act on message #{:07d};\tpriority #{}: {}.'.format(
message.get_number(), message.get_priority(),
message.get_description()))
if _current_message.get_event().is_ballistic:
self._log.info(
'acting upon accepted message with highest priority ballistic action #{}: {}'
.format(message.get_number(), message.get_description()))
self._log.info('waiting on ballistic action {}...'.format(
_current_message.get_event().description))
self._controller.act(_current_message,
self._action_complete_callback)
# then wait until completed
while not _current_message.is_complete():
self._log.info(
'loop: waiting on ballistic action {}...'.format(
_current_message.get_action().description))
time.sleep(self._ballistic_loop_delay_sec)
else:
self._log.info(
'acting upon accepted highest priority message #{}: {}'.format(
message.get_number(), message.get_description()))
self._controller.act(_current_message,
self._action_complete_callback)
# ..........................................................................
def _action_complete_callback(self, message, current_power):
'''
Callback from the Controller indicating that the message/action has been completed.
This sets the current message state to COMPLETED.
'''
# self._log.warning('1. event {}.'.format(message.get_event()))
# self._log.warning('2. current power at {:>5.1f}.'.format(current_power[0]))
# self._log.warning('3. current power at {:>5.1f}.'.format(current_power[1]))
_current_message = self._controller.get_current_message()
if _current_message:
if _current_message.is_complete():
self._log.warning('message already complete.')
return
elif current_power[0] is not None and current_power[1] is not None:
self._log.info(
'event {} complete with current power levels at {:>5.1f}, {:>5.1f}.'
.format(_current_message.get_event().name,
current_power[0], current_power[1]))
else:
self._log.info(
'event {} complete with current power levels at zero.'.
format(_current_message.get_event().name))
_current_message.complete()
else:
self._log.critical('cannot complete callback: no current message.')
# ..........................................................................
def add_task(self, task):
self._tasks.append(task)
task.start()
# ..........................................................................
def close(self):
self._is_enabled = False
if self._closing:
self._log.warning('already closing.')
return
elif self._closed:
self._log.warning('already closed.')
return
self._closing = False
if len(self._tasks) > 0:
self._log.info('closing {} tasks...'.format(len(self._tasks)))
for task in self._tasks:
task.disable()
task.close()
# # wait for thread exit
# task.join()
else:
self._log.info('no tasks to close.')
self._closed = False
self._log.info('closed.')
class Motor():
'''
Establishes power control over a motor using an encoder to determine
the velocity and distance traveled.
This uses the ros:motors: section of the configuration.
'''
def __init__(self, config, tb, pi, orientation, level):
global TB
super(Motor, self).__init__()
if config is None:
raise ValueError('null configuration argument.')
if tb is None:
raise ValueError('null thunderborg argument.')
self._tb = tb
TB = tb
if pi is None:
raise ValueError('null pi argument.')
self._pi = pi
# configuration ....................................
# get motors configuration section
cfg = config['ros'].get('motors')
# in case you wire something up backwards (we need this prior to the logger)
self._reverse_motor_orientation = cfg.get('reverse_motor_orientation')
# establish orientation
if self._reverse_motor_orientation:
self._orientation = Orientation.STBD if orientation is Orientation.PORT else Orientation.PORT
else:
self._orientation = orientation
# NOW we can create the logger
self._log = Logger('motor:{}'.format(orientation.label), level)
self._log.info('initialising {} motor...'.format(orientation))
self._log.debug('_reverse_motor_orientation: {}'.format(
self._reverse_motor_orientation))
self._reverse_encoder_orientation = cfg.get(
'reverse_encoder_orientation')
self._log.debug('_reverse_encoder_orientation: {}'.format(
self._reverse_encoder_orientation))
# GPIO pins configured for A1, B1, A2 and B2
self._rotary_encoder_a1_port = cfg.get(
'rotary_encoder_a1_port') # default: 22
self._log.debug('rotary_encoder_a1_port: {:d}'.format(
self._rotary_encoder_a1_port))
self._rotary_encoder_b1_port = cfg.get(
'rotary_encoder_b1_port') # default: 17
self._log.debug('rotary_encoder_b1_port: {:d}'.format(
self._rotary_encoder_b1_port))
self._rotary_encoder_a2_stbd = cfg.get(
'rotary_encoder_a2_stbd') # default: 27
self._log.debug('rotary_encoder_a2_stbd: {:d}'.format(
self._rotary_encoder_a2_stbd))
self._rotary_encoder_b2_stbd = cfg.get(
'rotary_encoder_b2_stbd') # default: 18
self._log.debug('rotary_encoder_b2_stbd: {:d}'.format(
self._rotary_encoder_b2_stbd))
# how many pulses per encoder measurement?
self._sample_rate = cfg.get('sample_rate') # default: 10
self._log.debug('sample_rate: {:d}'.format(self._sample_rate))
# convert raw velocity to approximate a percentage
self._velocity_fudge_factor = cfg.get(
'velocity_fudge_factor') # default: 14.0
self._log.debug('velocity fudge factor: {:>5.2f}'.format(
self._velocity_fudge_factor))
# limit set on power sent to motors
self._max_power_limit = cfg.get('max_power_limit') # default: 1.2
self._log.debug('maximum power limit: {:>5.2f}'.format(
self._max_power_limit))
# acceleration loop delay
self._accel_loop_delay_sec = cfg.get(
'accel_loop_delay_sec') # default: 0.10
self._log.debug('acceleration loop delay: {:>5.2f} sec'.format(
self._accel_loop_delay_sec))
# end configuration ................................
self._motor_power_limit = 0.99 # power limit to motor
self._steps = 0 # step counter
self._steps_begin = 0 # step count at beginning of velocity measurement
self._velocity = 0.0 # current velocity
self._max_velocity = 0.0 # capture maximum velocity attained
self._max_power = 0.0 # capture maximum power applied
self._max_driving_power = 0.0 # capture maximum adjusted power applied
self._interrupt = False # used to interrupt loops
self._stepcount_timestamp = time.time(
) # timestamp at beginning of velocity measurement
self._start_timestamp = time.time(
) # timestamp at beginning of velocity measurement
# configure encoder ................................
self._log.info('configuring rotary encoders...')
if self._reverse_encoder_orientation:
if orientation is Orientation.PORT:
self.configure_encoder(Orientation.STBD)
else:
self.configure_encoder(Orientation.PORT)
else:
self.configure_encoder(self._orientation)
self._log.info('ready.')
# ..............................................................................
@property
def velocity(self):
return self._velocity
# ..............................................................................
@property
def steps(self):
return self._steps
# ..............................................................................
def reset_steps(self):
self._steps = 0
# ..............................................................................
def set_max_power_ratio(self, max_power_ratio):
self._max_power_ratio = max_power_ratio
# ..............................................................................
def get_max_power_ratio(self):
return self._max_power_ratio
# ..............................................................................
def configure_encoder(self, orientation):
if self._orientation is Orientation.PORT:
ROTARY_ENCODER_A = self._rotary_encoder_a1_port
ROTARY_ENCODER_B = self._rotary_encoder_b1_port
elif self._orientation is Orientation.STBD:
ROTARY_ENCODER_A = self._rotary_encoder_a2_stbd
ROTARY_ENCODER_B = self._rotary_encoder_b2_stbd
else:
raise ValueError("unrecognised value for orientation.")
self._decoder = Decoder(self._pi, ROTARY_ENCODER_A, ROTARY_ENCODER_B,
self.callback_step_count)
self._log.info('configured {} rotary encoder on pin {} and {}.'.format(
orientation.name, ROTARY_ENCODER_A, ROTARY_ENCODER_B))
# ..............................................................................
def callback_step_count(self, pulse):
'''
This callback is used to capture encoder steps.
'''
if not self._reverse_encoder_orientation:
if self._orientation is Orientation.PORT:
self._steps = self._steps + pulse
else:
self._steps = self._steps - pulse
else:
if self._orientation is Orientation.PORT:
self._steps = self._steps - pulse
else:
self._steps = self._steps + pulse
if self._steps % self._sample_rate == 0:
if self._steps_begin != 0:
self._velocity = (
(self._steps - self._steps_begin) /
(time.time() - self._stepcount_timestamp) /
self._velocity_fudge_factor) # steps / duration
self._max_velocity = max(self._velocity, self._max_velocity)
self._stepcount_timestamp = time.time()
self._stepcount_timestamp = time.time()
self._steps_begin = self._steps
self._log.debug(
Fore.BLACK +
'{}: {:+d} steps'.format(self._orientation.label, self._steps))
# ..............................................................................
def cruise():
'''
Cruise at the current speed.
'''
pass # TODO
# ..........................................................................
def enable(self):
self._log.info('enabled.')
pass
# ..........................................................................
def disable(self):
self._log.info('disabled.')
pass
# ..........................................................................
def close(self):
self._log.info(
'max velocity: {:>5.2f}; max power: {:>5.2f}; max adjusted power: {:>5.2f}.'
.format(self._max_velocity, self._max_power,
self._max_driving_power))
self._log.info('closed.')
# ..........................................................................
def interrupt(self):
'''
Interrupt any loops by setting the _interrupt flag.
'''
self._interrupt = True
# ..........................................................................
def reset_interrupt(self):
'''
Reset the value of the _interrupt flag to False.
'''
self._interrupt = False
# ..........................................................................
def is_interrupted(self):
'''
Return the value of the _interrupt flag.
'''
return self._interrupt
# ..........................................................................
@staticmethod
def cancel():
'''
Stop both motors immediately. This can be called from either motor.
'''
try:
TB
except NameError:
TB = None
if TB:
TB.SetMotor1(0.0)
TB.SetMotor2(0.0)
else:
print('motor :' + Fore.YELLOW +
' WARN : cannot cancel motors: no thunderborg available.' +
Style.RESET_ALL)
# Motor Functions ............................................................
# ..........................................................................
def stop(self):
'''
Stops the motor immediately.
'''
self._log.info('stop.')
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
pass
# ..........................................................................
def halt(self):
'''
Quickly (but not immediately) stops.
'''
self._log.info('halting...')
# set slew slow, then decelerate to zero
self.accelerate(0.0, SlewRate.FAST, -1)
self._log.debug('halted.')
# ..........................................................................
def brake(self):
'''
Slowly coasts to a stop.
'''
self._log.info('braking...')
# set slew slow, then decelerate to zero
self.accelerate(0.0, SlewRate.SLOWER, -1)
self._log.debug('braked.')
# ..........................................................................
def ahead(self, speed):
'''
Slews the motor to move ahead at speed.
This is an alias to accelerate(speed).
'''
self._log.info('ahead to speed of {}...'.format(speed))
self.accelerate(speed, SlewRate.NORMAL, -1)
self._log.debug('ahead complete.')
# ..........................................................................
def stepAhead(self, speed, steps):
'''
Moves forwards specified number of steps, then stops.
'''
# self._log.info('step ahead {} steps to speed of {}...'.format(steps,speed))
self.accelerate(speed, SlewRate.NORMAL, steps)
# self._log.debug('step ahead complete.')
pass
# ..........................................................................
def astern(self, speed):
'''
Slews the motor to move astern at speed.
This is an alias to accelerate(-1 * speed).
'''
self._log.info('astern at speed of {}...'.format(speed))
self.accelerate(-1.0 * speed, SlewRate.NORMAL, -1)
self._log.debug('astern complete.')
# ..........................................................................
def stepAstern(self, speed, steps):
'''
Moves backwards specified number of steps, then stops.
'''
self._log.info('step astern {} steps to speed of {}...'.format(
steps, speed))
self.accelerate(speed, SlewRate.NORMAL, steps)
self._log.debug('step astern complete.')
pass
# ..........................................................................
def is_in_motion(self):
'''
Returns True if the motor is moving.
'''
return self.get_current_power_level() > 0.0
# ..........................................................................
def accelerate_to_velocity(self, velocity, slew_rate, steps):
'''
Slews the motor to the requested velocity.
If steps is greater than zero it provides a step limit.
'''
if steps > 0:
_step_limit = self._steps + steps
self._log.critical('>>>>>> {} steps, limit: {}.'.format(
self._steps, _step_limit))
else:
_step_limit = -1
if self._velocity == velocity: # if current velocity equals requested, no need to accelerate
self._log.info(
'NO CHANGE: ALREADY AT velocity {:>5.2f}/{:>5.2f}.'.format(
self._velocity, velocity))
return
# accelerate to target velocity...
self._accelerate_to_velocity(velocity, slew_rate, _step_limit)
self._log.info(Fore.YELLOW +
'REACHED TARGET VELOCITY: {:>5.2f}, now maintaining...'.
format(velocity))
self._log.info(Fore.BLUE + Style.BRIGHT +
'accelerated to velocity {:>5.2f} at power: {:>5.2f}. '.
format(velocity, self.get_current_power_level()))
# ..........................................................................
def set_motor_power(self, power_level):
'''
Sets the power level to a number between 0.0 and 1.0, with the
actual limits set both by the _max_driving_power limit and by
the _max_power_ratio, which alters the value to match the
power/motor voltage ratio.
'''
# safety checks ..........................
if power_level > self._motor_power_limit:
self._log.error(Style.BRIGHT +
'motor power too high: {:>5.2f}; limit: {:>5.2f}'.
format(power_level, self._motor_power_limit))
return
elif power_level < (-1.0 * self._motor_power_limit):
self._log.error(
Style.BRIGHT +
'motor power too low: {:>5.2f}; limit: {:>5.2f}'.format(
power_level, (-1.0 * self._motor_power_limit)))
return
_current_power = self.get_current_power_level()
# _current_actual_power = _current_power * ( 1.0 / self._max_power_ratio )
if abs(_current_power -
power_level) > 0.3 and _current_power > 0.0 and power_level < 0:
self._log.error(
'cannot perform positive-negative power jump: {:>5.2f} to {:>5.2f}.'
.format(_current_power, power_level))
return
elif abs(_current_power - power_level
) > 0.3 and _current_power < 0.0 and power_level > 0:
self._log.error(
'cannot perform negative-positive power jump: {:>5.2f} to {:>5.2f}.'
.format(_current_power, power_level))
return
# okay, let's go .........................
_driving_power = float(power_level * self._max_power_ratio)
self._max_power = max(power_level, self._max_power)
self._max_driving_power = max(abs(_driving_power),
self._max_driving_power)
self._log.debug(Fore.MAGENTA + Style.BRIGHT + 'power argument: {:>5.2f}'.format(power_level) + Style.NORMAL \
+ '\tcurrent power: {:>5.2f}; driving power: {:>5.2f}.'.format(_current_power, _driving_power))
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(_driving_power)
else:
self._tb.SetMotor2(_driving_power)
# ..........................................................................
@property
def orientation(self):
'''
Returns the orientation of this motor.
'''
return self._orientation
# ..........................................................................
def is_stopped(self):
'''
Returns true if the motor is entirely stopped.
'''
return (self.get_current_power_level() == 0.0)
# ................................
def get_current_power_level(self):
'''
Makes a best attempt at getting the power level value from the motors.
'''
value = None
count = 0
if self._orientation is Orientation.PORT:
while value == None and count < 20:
count += 1
value = self._tb.GetMotor1()
time.sleep(0.005)
else:
while value == None and count < 20:
count += 1
value = self._tb.GetMotor2()
time.sleep(0.005)
if value == None:
return 0.0
else:
return value
# ..........................................................................
def accelerate(self, speed, slew_rate, steps):
'''
Slews the motor to the designated speed. -100 <= 0 <= speed <= 100.
This takes into account the maximum power to be supplied to the
motor based on the battery and motor voltages.
If steps > 0 then run until the number of steps has been reached.
'''
self._interrupt = False
_current_power_level = self.get_current_power_level()
if _current_power_level is None:
raise RuntimeError(
'cannot continue: unable to read current power from motor.')
self._log.info(
'current power: {:>5.2f} max power ratio: {:>5.2f}...'.format(
_current_power_level, self._max_power_ratio))
_current_power_level = _current_power_level * (1.0 /
self._max_power_ratio)
# accelerate to desired speed
_desired_div_100 = float(speed / 100)
self._log.info('accelerating from {:>5.2f} to {:>5.2f}...'.format(
_current_power_level, _desired_div_100))
if _current_power_level == _desired_div_100: # no change
self._log.warning(
'already at acceleration power of {:>5.2f}, exiting.'.format(
_current_power_level))
return
elif _current_power_level < _desired_div_100: # moving ahead
_slew_rate_ratio = slew_rate.ratio
overstep = 0.001
else: # moving astern
_slew_rate_ratio = -1.0 * slew_rate.ratio
overstep = -0.001
# self._log.warning(Style.BRIGHT + 'LOOP from {:>5.2f} to limit: {:>5.2f} with slew: {:>5.2f}'.format(_current_power_level, (_desired_div_100 + overstep), _slew_rate_ratio))
driving_power_level = 0.0
for step_power in numpy.arange(_current_power_level,
(_desired_div_100 + overstep),
_slew_rate_ratio):
driving_power_level = float(step_power * self._max_power_ratio)
self.set_motor_power(driving_power_level)
if self._interrupt:
break
time.sleep(self._accel_loop_delay_sec)
# be sure we're powered off
if speed == 0.0 and abs(driving_power_level) > 0.00001:
self._log.warning(
'non-zero power level: {:7.5f}v; stopping completely...'.
format(driving_power_level))
if self._orientation is Orientation.PORT:
self._tb.SetMotor1(0.0)
else:
self._tb.SetMotor2(0.0)
self._log.debug('accelerate complete.')
class Motors():
'''
A dual motor controller with encoders.
'''
def __init__(self, config, tb, level):
super().__init__()
self._log = Logger('motors', level)
self._log.info('initialising motors...')
if tb is None:
tb = self._configure_thunderborg_motors(level)
if tb is None:
raise Exception('unable to configure thunderborg.')
self._tb = tb
self._set_max_power_ratio()
self._pi = pigpio.pi()
self._port_motor = Motor(config, self._tb, self._pi, Orientation.PORT,
level)
self._port_motor.set_max_power_ratio(self._max_power_ratio)
self._stbd_motor = Motor(config, self._tb, self._pi, Orientation.STBD,
level)
self._stbd_motor.set_max_power_ratio(self._max_power_ratio)
self._enabled = True # default enabled
# a dictionary of motor # to last set value
self._msgIndex = 0
self._last_set_power = {0: 0, 1: 0}
self._log.info('motors ready.')
# ..........................................................................
def _configure_thunderborg_motors(self, level):
'''
Import the ThunderBorg library, then configure the Motors.
'''
self._log.info('configure thunderborg & motors...')
global pi
try:
self._log.info('importing thunderborg...')
import lib.ThunderBorg3 as ThunderBorg
self._log.info('successfully imported thunderborg.')
TB = ThunderBorg.ThunderBorg(
level) # 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:'
.format(TB.i2cAddress))
for board in boards:
self._log.info('board {:02x} {:d}'.format(
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)
return TB
except Exception as e:
self._log.error('unable to import thunderborg: {}'.format(e))
traceback.print_exc(file=sys.stdout)
sys.exit(1)
# ..........................................................................
def set_led_show_battery(self, enable):
self._tb.SetLedShowBattery(enable)
# ..........................................................................
def set_led_color(self, color):
self._tb.SetLed1(color.red / 255.0, color.green / 255.0,
color.blue / 255.0)
# ..........................................................................
def _set_max_power_ratio(self):
pass
# initialise ThunderBorg ...........................
self._log.info('getting battery reading...')
# get battery voltage to determine max motor power
# could be: Makita 12V or 18V power tool battery, or 12-20V line supply
voltage_in = self._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:
self._max_power_ratio = 1.0
else:
self._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;'.format( voltage_in, voltage_out) + Fore.CYAN + Style.BRIGHT \
+ ' maximum power ratio: {}'.format(str(Fraction(self._max_power_ratio).limit_denominator(max_denominator=20)).replace('/',':')))
# ..........................................................................
def get_motor(self, orientation):
if orientation is Orientation.PORT:
return self._port_motor
else:
return self._stbd_motor
# ..........................................................................
def enable(self):
self._enabled = True
# ..........................................................................
def disable(self):
'''
Disable the motors, halting first if in motion.
'''
self._log.info('disabling motors...')
self._enabled = False
if self.is_in_motion(): # if we're moving then halt
self._log.warning('event: motors are in motion (halting).')
self.halt()
self._log.info('motors disabled.')
# ..........................................................................
def is_in_motion(self):
'''
Returns true if either motor is moving.
'''
return self._port_motor.is_in_motion(
) or self._stbd_motor.is_in_motion()
# ..........................................................................
def is_faster_than(self, speed):
'''
Returns true if either motor is moving faster than the argument.
DEAD_SLOW_SPEED = 20.0
SLOW_SPEED = 30.0
HALF_SPEED = 50.0
THREE_QUARTER_SPEED = 65.0
FULL_SPEED = 80.0
EMERGENCY_SPEED = 100.0
MAXIMUM = 100.000001
'''
self._log.warning('SPEED {:5.2f} compared to port: {:>5.2f}; stbd: {:>5.2f}'.format(speed.value, self._port_motor.get_current_power_level(), \
self._stbd_motor.get_current_power_level()) )
return (self._port_motor.get_current_power_level() > speed.value) or (
self._stbd_motor.get_current_power_level() > speed.value)
# ..........................................................................
def get_steps(self):
'''
Returns the port and starboard motor step count.
'''
return [self._port_motor.get_steps(), self._stbd_motor.get_steps()]
# ..........................................................................
def get_current_power_level(self, orientation):
'''
Returns the last set power of the specified motor.
'''
if orientation is Orientation.PORT:
return self._port_motor.get_current_power_level()
else:
return self._stbd_motor.get_current_power_level()
# ..........................................................................
def close(self):
'''
Halts, turn everything off and stop doing anything.
'''
self._log.debug('closing...')
self.halt()
self._port_motor.close()
self._stbd_motor.close()
self._log.debug('closed.')
# Stopping Behaviours ....................................................................
# ..........................................................................
def interrupt(self):
'''
Interrupt any motor loops by setting the _interrupt flag.
'''
self._port_motor.interrupt()
self._stbd_motor.interrupt()
# ..........................................................................
def halt(self):
'''
Quickly (but not immediately) stops both motors.
'''
self._log.info('halting...')
if self.is_stopped():
self._log.debug('already stopped.')
return True
# source: https://stackoverflow.com/questions/2957116/make-2-functions-run-at-the-same-time
_tp = Thread(target=self.processStop,
args=(Event.HALT, Orientation.PORT))
_ts = Thread(target=self.processStop,
args=(Event.HALT, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self._log.info('halted.')
return True
# ..........................................................................
def brake(self):
'''
Slowly coasts both motors to a stop.
'''
self._log.info('braking...')
if self.is_stopped():
self._log.warning('already stopped.')
return True
_tp = Thread(target=self.processStop,
args=(Event.BRAKE, Orientation.PORT))
_ts = Thread(target=self.processStop,
args=(Event.BRAKE, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self._log.info('braked.')
return True
# ..........................................................................
def stop(self):
'''
Stops both motors immediately, with no slewing.
'''
self._log.info('stopping...')
if self.is_stopped():
self._log.warning('already stopped.')
return True
self._port_motor.stop()
self._stbd_motor.stop()
self._log.info('stopped.')
return True
# # ..........................................................................
# def slow_down(self, orientation):
# '''
# Slows both motors one step in the Velocity enumeration.
# '''
# if not self.is_in_motion():
# self._log.warning('not moving: can\'t slow down.')
# else:
# self._log.info('slowing...')
# _port_velocity = self._port_motor.get_velocity()
# _slowed_port_velocity = Velocity.get_slower_than(_port_velocity).value
# self._log.info(Fore.RED + 'slowing port motor from {:>5.2f} to {:>5.2f}...'.format(_port_velocity, _slowed_port_velocity))
# _stbd_velocity = self._stbd_motor.get_velocity()
# _slowed_stbd_velocity = Velocity.get_slower_than(_stbd_velocity).value
# self._log.info(Fore.GREEN + 'slowing stbd motor from {:>5.2f} to {:>5.2f}...'.format(_stbd_velocity, _slowed_stbd_velocity))
# _forward_steps_per_rotation = 494
# _rotations = 2
# _steps = -1 #_forward_steps_per_rotation * _rotations
# self.change_velocity(_slowed_port_velocity, _slowed_stbd_velocity, SlewRate.FAST, _steps)
# self._log.info('slowed.')
# return True
# ..........................................................................
def is_stopped(self):
return self._port_motor.is_stopped() and self._stbd_motor.is_stopped()
# Synchronisation Support ................................................................
# ..........................................................................
def processStop(self, event, orientation):
'''
Synchronised process control over various kinds of stopping.
'''
if orientation is Orientation.PORT:
if event is Event.HALT:
self._log.info('halting port motor...')
self._port_motor.halt()
elif event is Event.BRAKE:
self._log.info('braking port motor...')
self._port_motor.brake()
else: # is stop
self._log.info('stopping port motor...')
self._port_motor.stop()
else:
if event is Event.HALT:
self._log.info('halting starboard motor...')
self._stbd_motor.halt()
elif event is Event.BRAKE:
self._log.info('braking starboard motor...')
self._stbd_motor.brake()
else: # is stop
self._log.info('stopping starboard motor...')
self._stbd_motor.stop()
self.print_current_power_levels()
# ..........................................................................
def get_current_power_levels(self):
'''
Returns the last set power values.
'''
_port_power = self._port_motor.get_current_power_level()
_stbd_power = self._stbd_motor.get_current_power_level()
return [_port_power, _stbd_power]
# ..........................................................................
def print_current_power_levels(self):
'''
Prints the last set power values.
'''
self._msgIndex += 1
self._log.info('{}:\tcurrent power:\t{:6.1f}\t{:6.1f}'.format(
self._msgIndex, self._last_set_power[0], self._last_set_power[1]))
# ..........................................................................
def accelerate(self, speed, slew_rate, steps, orientation):
'''
Unsynchronised (non-threaded) process control for a single motor. This
is generally called by a dual-thread process to control both motors.
'''
if not self._enabled:
self._log.info('cannot accelerate: motors disabled.')
return
self._log.debug('accelerating...')
if orientation is Orientation.PORT:
self._log.info(
'starting port motor with {:>5.2f} speed for {:d} steps...'.
format(speed, steps))
self._port_motor.accelerate(speed, slew_rate, steps)
else:
self._log.info(
'starting starboard motor with {:>5.2f} speed for {:d} steps...'
.format(speed, steps))
self._stbd_motor.accelerate(speed, slew_rate, steps)
self._log.debug('accelerated.')
# Straight Movement Behaviours ...........................................................
# ..........................................................................
def accelerate_to_zero(self, slew_rate):
'''
Slows both motors to a stop at the provided slew rate.
'''
if not self._enabled:
self._log.info('cannot change velocity: motors disabled.')
return
self._log.info('slow to zero...')
_tp = Thread(target=self._accelerate_to_zero,
args=(slew_rate, Orientation.PORT))
_ts = Thread(target=self._accelerate_to_zero,
args=(slew_rate, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
# self.print_current_power_levels()
self._log.info('motors slow to zero complete.')
return True
# ..........................................................................
def _accelerate_to_zero(self, slew_rate, orientation):
'''
Synchronised process control for both motors, to slow to a stop.
'''
if not self._enabled:
self._log.info('cannot continue: motors disabled.')
return
if orientation is Orientation.PORT:
self._log.debug('slowing port motor to a stop...')
self._port_motor.accelerate_to_zero(slew_rate)
else:
self._log.debug('slowing starboard motor to a stop...')
self._stbd_motor.accelerate_to_zero(slew_rate)
self._log.debug('accelerated.')
# ..........................................................................
def change_velocity(self, port_velocity, stbd_velocity, slew_rate, steps):
'''
Slews both motors to the designated velocities at the provided slew rate.
If steps is greater than zero it provides a step limit on the motors.
'''
# https://stackoverflow.com/questions/2957116/make-2-functions-run-at-the-same-time
# https://github.com/ray-project/ray
if not self._enabled:
self._log.info('cannot change velocity: motors disabled.')
return
self._log.info(
'change ahead quickly to velocities; port: {:>5.2f}; stbd: {:>5.2f}.'
.format(port_velocity, stbd_velocity))
_tp = Thread(target=self._accelerate_to_velocity,
args=(port_velocity, slew_rate, steps, Orientation.PORT))
_ts = Thread(target=self._accelerate_to_velocity,
args=(stbd_velocity, slew_rate, steps, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
# self.print_current_power_levels()
self._log.info('motors change velocity complete.')
return True
# ..........................................................................
def _accelerate_to_velocity(self, velocity, slew_rate, steps, orientation):
'''
Synchronised process control for both motors, over various kinds of accelerating.
'''
if not self._enabled:
self._log.info('cannot accelerate: motors disabled.')
return
self._log.debug(
'accelerating to velocity {:>6.3f}...'.format(velocity))
if orientation is Orientation.PORT:
self._log.debug(
'starting port motor with {:>5.2f} velocity...'.format(
velocity))
self._port_motor.accelerate_to_velocity(velocity, slew_rate, steps)
else:
self._log.debug(
'starting starboard motor with {:>5.2f} velocity...'.format(
velocity))
self._stbd_motor.accelerate_to_velocity(velocity, slew_rate, steps)
self._log.debug('accelerated.')
# ..........................................................................
def step_to(self, steps):
'''
Maintains the current velocity and runs until the number of steps have been reached.
The argument is in absolute steps, not relative to the beginning of the method call.
'''
if not self._enabled:
self._log.info('cannot step ahead: motors disabled.')
return
self._log.info('change ahead quickly to {:d} steps.'.format(steps))
_tp = Thread(target=self._step_to, args=(steps, Orientation.PORT))
_ts = Thread(target=self._step_to, args=(steps, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
# self.print_current_power_levels()
self._log.info('motors step to complete.')
return True
# ..........................................................................
def _step_to(self, steps, orientation):
'''
Steps the specified motor to a specific step location.
'''
if not self._enabled:
self._log.info('cannot step to: motors disabled.')
elif orientation is Orientation.PORT:
self._log.info(
'advancing port motor to {:d} steps...'.format(steps))
self._port_motor.step_to(steps)
else:
self._log.info(
'advancing starboard motor to {:d} steps...'.format(steps))
self._stbd_motor.step_to(steps)
# =======================================================================================================
# ..........................................................................
def ahead(self, speed):
'''
Slews both motors to move ahead at speed. 0 <= speed <= 100.
'''
if not self._enabled:
self._log.info('cannot move ahead: motors disabled.')
return
self._log.info('motors ahead at speed {:6.3f}...'.format(speed))
self.step(speed, speed, -1, -1)
self._log.info('motors ahead complete.')
return True
# ..........................................................................
def ahead_for_steps(self, port_speed, stbd_speed, port_steps, stbd_steps):
'''
An experimental method that slews both motors to move ahead at speed (0 <= speed <= 100)
for a specified number of steps, stopping at the end. As this provides no slewing at the
end to avoid stress on the motors' gears do not use with a high speed.
'''
if self._enabled:
self._log.info(
'motors ahead at speed {:5.2f}/{:5.2f} for steps {:d}/{:d}...'.
format(port_speed, stbd_speed, port_steps, stbd_steps))
_tp = Thread(target=self._port_motor.ahead_for_steps,
args=(port_speed, port_steps))
_ts = Thread(target=self._stbd_motor.ahead_for_steps,
args=(stbd_speed, stbd_steps))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
# self.stop()
self.print_current_power_levels()
# while _tp.is_alive() and _ts.is_alive():
# time.sleep(0.001)
self._log.info('motors ahead for steps complete.')
return True
else:
self._log.info('cannot move ahead: motors disabled.')
return False
# ..........................................................................
def change_speed(self, speed):
'''
Slews both motors to the provided speed at a higher slew rate than 'ahead()'.
'''
if not self._enabled:
self._log.info('cannot change speed: motors disabled.')
return
self._log.info('change ahead quickly to speed {:6.3f}.'.format(speed))
_slew_rate = SlewRate.FAST
_tp = Thread(target=self.accelerate,
args=(speed, _slew_rate, -1, Orientation.PORT))
_ts = Thread(target=self.accelerate,
args=(speed, _slew_rate, -1, Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self.print_current_power_levels()
self._log.info('motors change speed complete.')
return True
# ..........................................................................
def astern(self, speed):
'''
Slews both motors astern at Speed, using the enum.
The value of the enum is: 0 <= speed <= 100.
'''
if not self._enabled:
self._log.info('cannot move astern: motors disabled.')
return
self._log.critical('motors astern at speed {:6.3f}...'.format(speed))
self.step(-1.0 * speed, -1.0 * speed, -1, -1)
# self.step(-1.0 * speed, -1.0 * speed, -1, -1)
# _slew_rate = SlewRate.NORMAL
# _tp = Thread(target=self.accelerate, args=(-1.0 * speed, _slew_rate, -1, Orientation.PORT))
# _ts = Thread(target=self.accelerate, args=(-1.0 * speed, _slew_rate, -1, Orientation.STBD))
# _tp.start()
# _ts.start()
# _tp.join()
# _ts.join()
self._log.critical('motors astern complete.')
return True
# ..........................................................................
def stepAstern(self, speed, steps):
'''
Slews both motors astern the specified number of steps, then stops.
'''
if not self._enabled:
self._log.info('cannot step astern: motors disabled.')
return
self._log.info('motors astern {} steps to speed of {:6.3f}...'.format(
steps, speed))
self.step(-1.0 * speed, -1.0 * speed, steps, steps)
self._log.info('motors step astern complete.')
return True
# ..........................................................................
def stepAhead(self, speed, steps):
'''
Slews ahead or astern the specified number of steps, then stops.
'''
if not self._enabled:
self._log.info('cannot step ahead: motors disabled.')
return
self._log.info('motors ahead {} steps to speed of {:6.3f}...'.format(
steps, speed))
self.step(speed, speed, steps, steps)
self._log.info('motors step ahead complete.')
return True
# Turning Behaviours .....................................................................
# ..........................................................................
def turn_ahead(self, port_speed, stbd_speed):
'''
Moves ahead in an arc by setting different speeds. 0 <= port_speed,starboard_speed <= 100
If the port speed is greater than the starboard the robot will curve to starboard (clockwise).
If the starboard speed is greater than the port the robot will curve to port (counter-clockwise).
'''
if not self._enabled:
self._log.info('cannot turn ahead: motors disabled.')
return
self._log.info(
'turning with port speed {:6.3f} and starboard speed {:6.3f}.'.
format(port_speed, stbd_speed))
self.step(abs(port_speed), abs(stbd_speed), -1, -1)
self._log.info('turned ahead.')
return True
# ..........................................................................
def turn_astern(self, port_speed, stbd_speed):
'''
Moves astern in an arc by setting different speeds. 0 <= port_speed,starboard_speed <= 100
If the port speed is greater than the starboard the robot will curve to starboard (clockwise).
If the starboard speed is greater than the port the robot will curve to port (counter-clockwise).
'''
if not self._enabled:
self._log.info('cannot turn astern: motors disabled.')
return
self._log.info(
'turning astern with port speed {:6.3f} and starboard speed {:6.3f}.'
.format(port_speed, stbd_speed))
self.step(-1.0 * abs(port_speed), -1.0 * abs(stbd_speed), -1, -1)
self._log.info('turned astern.')
return True
# ..........................................................................
def stepTurnAstern(self, port_speed, stbd_speed, port_steps, stbd_steps):
'''
Turns astern using the port and starboard speeds, going the number of port and starboard steps before stopping.
If a step argument is -1 no step limit is set.
If the port speed is greater than the starboard the robot will curve to starboard (clockwise).
If the starboard speed is greater than the port the robot will curve to port (counter-clockwise).
'''
if not self._enabled:
self._log.info('cannot step turn astern: motors disabled.')
return
self._log.info(
'astern turning with port speed {:6.3f} for {} steps and starboard speed {:6.3f} for {} steps.'
.format(port_speed, port_steps, stbd_speed, stbd_steps))
self.step(-1.0 * abs(port_speed), -1.0 * abs(stbd_speed), port_steps,
stbd_steps)
self._log.info('step turned astern.')
return True
# ..........................................................................
def step(self, port_speed, stbd_speed, port_steps, stbd_steps):
'''
Moves ahead or backward using the designated port and starboard speeds, going the number
of port and starboard steps before stopping. If a step argument is -1 no step limit is set.
If the speeds are equal the robot will move ahead or astern in a straight line.
If the port speed is greater than the starboard the robot will curve to starboard (clockwise).
If the starboard speed is greater than the port the robot will curve to port (counter-clockwise).
This is the method that actually does all the work.
'''
if not self._enabled:
self._log.info('cannot step: motors disabled.')
return
self._log.info(
'step with port speed {:6.3f} for {} steps and starboard speed {:6.3f} for {} steps.'
.format(port_speed, port_steps, stbd_speed, stbd_steps))
_slew_rate = SlewRate.NORMAL
_tp = Thread(target=self.accelerate,
args=(port_speed, _slew_rate, port_steps,
Orientation.PORT))
_ts = Thread(target=self.accelerate,
args=(stbd_speed, _slew_rate, stbd_steps,
Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self.print_current_power_levels()
self._log.info('step complete.')
# Spinning Behaviours ....................................................................
# ..........................................................................
def spin_port(self, speed):
'''
Halts, then sets motors to turn counter-clockwise at speed. 0 <= speed <= 100
'''
if not self._enabled:
self._log.info('cannot spin to port: motors disabled.')
return
self._log.info('spinning to port at speed {:6.3f}...'.format(speed))
self.step_spin(Orientation.PORT, speed, -1, False)
self._log.info('spun to port.')
return True
# ..........................................................................
def spin_starboard(self, speed):
'''
Halts, then sets motors to turn clockwise at speed. 0 <= speed <= 100
'''
if not self._enabled:
self._log.info('cannot spin to starboard: motors disabled.')
return
self._log.info(
'spinning to starboard at speed {:6.3f}...'.format(speed))
self.step_spin(Orientation.STBD, speed, -1, False)
self._log.info('spun to starboard.')
return True
# ..........................................................................
def step_spin(self, orientation, speed, steps, halt_first):
'''
Halts, then spins to port (counter-clockwise) or starboard (clockwise)
at the specified speed and steps, then stops.
'''
if not self._enabled:
self._log.info('cannot step spin to port: motors disabled.')
return
if halt_first:
self.halt()
_slew_rate = SlewRate.NORMAL
if orientation is Orientation.PORT:
self._log.info(
'spinning to port {} steps at speed {:6.3f}...'.format(
steps, speed))
_port_speed = abs(speed)
_starboard_speed = -1.0 * abs(speed)
else:
self._log.info(
'spinning to starboard {} steps at speed {:6.3f}...'.format(
steps, speed))
_port_speed = -1.0 * abs(speed)
_starboard_speed = abs(speed)
_tp = Thread(target=self.accelerate,
args=(_port_speed, _slew_rate, steps, Orientation.PORT))
_ts = Thread(target=self.accelerate,
args=(_starboard_speed, _slew_rate, steps,
Orientation.STBD))
_tp.start()
_ts.start()
_tp.join()
_ts.join()
self.print_current_power_levels()
if orientation is Orientation.PORT:
self._log.info('step spun to port.')
else:
self._log.info('step spun to starboard.')
return True
# ..........................................................................
@staticmethod
def cancel():
print('cancelling motors...')
Motor.cancel()
class Controller():
'''
Responds to Events. Simple tasks are handled within this script, more
complicated ones are farmed out to task files.
There are two API methods for this class:
.get_current_message() returns the last message received
.act(_current_message, _action_complete_callback)
act upon the current message, with
a callback called upon completion
'''
def __init__(self, config, ifs, motors, callback_shutdown, level):
super().__init__()
self._log = Logger('controller', level)
self._config = config
self._enable_self_shutdown = self._config['ros'].get(
'enable_self_shutdown')
# self._switch = switch
self._ifs = ifs
self._motors = motors
self._port_motor = motors.get_motor(Orientation.PORT)
self._port_pid = self._port_motor.get_pid_controller()
self._stbd_motor = motors.get_motor(Orientation.STBD)
self._stbd_pid = self._stbd_motor.get_pid_controller()
self._callback_shutdown = callback_shutdown
self._status = Status(config, GPIO, level)
self._current_message = None
self._standby = False
self._enabled = True
# behaviours .................................
self._behaviours = Behaviours(motors, ifs, Level.INFO)
self._roam_behaviour = RoamBehaviour(motors, Level.INFO)
self._log.debug('ready.')
# ..........................................................................
def set_standby(self, is_standby):
if is_standby:
self._log.info('standby.')
self._status.blink(True)
self._motors.disable()
# self._switch.off()
self._ifs.disable()
self._standby = True
else:
self._log.info('active (standby off).')
self._status.blink(False)
self._motors.enable()
# self._switch.on()
self._ifs.enable()
self._status.enable()
self._standby = False
# ................................................................
def enable(self):
self._enabled = True
self._status.enable()
self._log.info('enabled.')
# ................................................................
def disable(self):
self._enabled = False
self._status.disable()
self._log.info('disabled.')
# ..........................................................................
def get_current_message(self):
return self._current_message
# ..........................................................................
def _clear_current_message(self):
self._log.debug('clear current message {}.'.format(
self._current_message))
self._current_message = None
# ..........................................................................
def _slow_down(self, orientation):
self._log.info(Fore.CYAN + 'slow down {}.'.format(orientation.name))
self._motors.slow_down(orientation)
pass
# ..........................................................................
def _avoid(self, orientation):
self._log.info(Fore.CYAN + 'avoid {}.'.format(orientation.name))
self._behaviours.avoid(orientation)
# ..........................................................................
def act(self, message, callback):
'''
Responds to the Event contained within the Message.
The callback method's API is:
callback(self._current_message, _current_power_levels)
'''
if not self._enabled:
self._log.warning('action ignored: controller disabled.')
return
_start_time = dt.datetime.now()
self._current_message = message
# _port_speed = self._motors.get_current_power_level(Orientation.PORT)
# _stbd_speed = self._motors.get_current_power_level(Orientation.STBD)
# if _port_speed is not None and _stbd_speed is not None:
# self._log.debug('last set power port: {:6.1f}, starboard: {:6.1f}'.format(_port_speed, _stbd_speed))
# else:
# self._log.debug('last set power port: {}, starboard: {}'.format(_port_speed, _stbd_speed))
_event = self._current_message.get_event()
self._log.debug(Fore.CYAN + 'act()' + Style.BRIGHT +
' event: {}.'.format(_event) + Fore.YELLOW)
self._current_message.start()
# no action ............................................................
if _event is Event.NO_ACTION:
self._log.info('event: no action.')
pass
# BUTTON ..............................................
# button ...............................................................
elif _event is Event.BUTTON:
# if button pressed we change standby mode but don't do anything else
_value = self._current_message.get_value()
# toggle value
# self.set_standby(not _value)
if _value:
self._log.critical('event: button value: {}.'.format(_value))
self.set_standby(False)
else:
self._log.error('event: button value: {}.'.format(_value))
self.set_standby(True)
# if in standby mode we don't process the event, but we do perform the callback
# # stopping and halting ...................................................
# SHUTDOWN ..............................................
# shutdown ............................................................
elif _event is Event.SHUTDOWN:
self._log.info('event: shutdown.')
self._motors.stop()
self._callback_shutdown()
# STOP ..............................................
# stop .................................................................
elif _event is Event.STOP:
self._log.info('event: stop.')
self._motors.stop()
# HALT ..............................................
# halt .................................................................
elif _event is Event.HALT:
self._log.info('event: halt.')
self._motors.halt()
# BRAKE ...................................
# brake ................................................................
elif _event is Event.BRAKE:
self._log.info('event: brake.')
self._motors.brake()
# STANDBY ..............................................
# standby ..............................................................
elif _event is Event.STANDBY:
# toggle standby state
_value = self._current_message.get_value()
if _value == 1:
if self._standby:
self.set_standby(False)
else:
self.set_standby(True)
else:
pass
# ROAM .................................................
elif _event is Event.ROAM:
self._log.info('event: roam.')
self._roam_behaviour.roam()
# SNIFF .................................................
elif _event is Event.SNIFF:
self._log.info('event: sniff.')
self._behaviours.sniff()
time.sleep(0.5) # debounce gamepad
# D-PAD HORIZONTAL .................................................
elif _event is Event.FORWARD_VELOCITY:
_direction = message.get_value()
_speed = 80.0
if _direction == -1: # then ahead
self._log.info('event: d-pad movement: ahead.')
# self._motors.change_velocity(0.5, 0.5, SlewRate.SLOW, -1)
self._motors.ahead(_speed)
time.sleep(2.0)
elif _direction == 1: # then astern
self._log.info('event: d-pad movement: astern.')
# self._motors.change_velocity(-0.5, -0.5, SlewRate.SLOW, -1)
self._motors.astern(_speed)
time.sleep(2.0)
else:
self._log.info('event: d-pad movement: halt.')
# D-PAD VERTICAL .................................................
elif _event is Event.THETA:
_direction = message.get_value()
if _direction == -1: # then ahead
self._log.info('event: d-pad rotate: counter-clockwise.')
self._motors.step(-100.0, 100.0, -1, -1)
time.sleep(2.0)
elif _direction == 1: # then astern
self._log.info('event: d-pad rotate: clockwise.')
self._motors.step(100.0, -100.0, -1, -1)
time.sleep(2.0)
else:
self._log.info('event: d-pad rotate: none.')
# PORT_VELOCITY ................................................
elif _event is Event.PORT_VELOCITY:
_velocity = Gamepad.convert_range(message.get_value())
self._log.info(
Fore.GREEN + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
self._port_motor.set_motor_power(_velocity)
# PORT_THETA ...................................................
elif _event is Event.PORT_THETA:
_velocity = -1 * Gamepad.convert_range(message.get_value())
self._log.info(
Fore.MAGENTA + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
# STBD_VELOCITY ................................................
elif _event is Event.STBD_VELOCITY:
_velocity = Gamepad.convert_range(message.get_value())
self._log.info(
Fore.GREEN + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
self._stbd_motor.set_motor_power(_velocity)
# STBD_THETA ...................................................
elif _event is Event.STBD_THETA:
_velocity = -1 * Gamepad.convert_range(message.get_value())
self._log.info(
Fore.MAGENTA + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
# # bumper .............................................................
# BUMPER_PORT ..............................................
elif _event is Event.BUMPER_PORT:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._ifs.disable()
self._motors.stop()
# self._motors.astern(Speed.HALF.value)
self._motors.turn_astern(Speed.THREE_QUARTER.value,
Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._ifs.enable()
self._log.info('action complete: port bumper.')
else:
self._log.info('no action required (not moving): port bumper.')
# BUMPER_CNTR ..............................................
elif _event is Event.BUMPER_CNTR:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._ifs.disable()
self._motors.stop()
self._motors.astern(Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._ifs.enable()
self._log.info('action complete: center bumper.')
else:
self._log.info(
'no action required (not moving): center bumper.')
# BUMPER_STBD ..............................................
elif _event is Event.BUMPER_STBD:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._motors.stop()
# self._motors.astern(Speed.FULL.value)
self._motors.turn_astern(Speed.HALF.value,
Speed.THREE_QUARTER.value)
time.sleep(0.5)
self._motors.brake()
self._log.info('action complete: starboard bumper.')
else:
self._log.info(
'no action required (not moving): starboard bumper.')
# # infrared ...........................................................
# INFRARED_PORT_SIDE ..............................................
elif _event is Event.INFRARED_PORT_SIDE:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port side infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
pass
# INFRARED_PORT ..............................................
elif _event is Event.INFRARED_PORT:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.PORT)
self._log.info('action complete: port infrared.')
else:
self._log.info(
'no action required (not moving): port infrared.')
pass
# INFRARED_CNTR ..............................................
elif _event is Event.INFRARED_CNTR:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.CNTR)
self._log.info('action complete: center infrared.')
else:
self._log.info(
'no action required (not moving): center infrared.')
pass
# INFRARED_STBD ...................................................
elif _event is Event.INFRARED_STBD:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.STBD)
self._log.info('action complete: starboard infrared.')
else:
self._log.info(
'no action required (not moving): starboard infrared.')
pass
# INFRARED_STBD_SIDE ...........................................
elif _event is Event.INFRARED_STBD_SIDE:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard side infrared; ' +
Fore.YELLOW + 'value: {}'.format(_value))
pass
# EVENT UNKNOWN: FAILURE ...............................................
else:
self._log.error('unrecognised event: {}'.format(_event))
pass
_current_power_levels = self._motors.get_current_power_levels()
if callback is not None:
self._log.debug(
Fore.MAGENTA + 'callback message: {}; '.format(
self._current_message.get_value()) + Fore.CYAN +
'current power levels: {}'.format(_current_power_levels))
callback(self._current_message, _current_power_levels)
self._clear_current_message()
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.debug(Fore.MAGENTA + Style.DIM +
'elapsed: {}ms'.format(_elapsed_ms) + Style.DIM)
class Controller():
'''
Responds to Events.
'''
def __init__(self, level, config, switch, infrared_trio, motors, rgbmatrix,
lidar, callback_shutdown):
super().__init__()
self._log = Logger('controller', level)
self._config = config
self._enable_self_shutdown = self._config['ros'].get(
'enable_self_shutdown')
self._switch = switch
# self._info = info
self._infrared_trio = infrared_trio
self._motors = motors
self._rgbmatrix = rgbmatrix
self._lidar = lidar
self._callback_shutdown = callback_shutdown
self._status = Status(GPIO, level)
self._current_message = None
self._standby = False
self._log.debug('ready.')
# ..........................................................................
def set_standby(self, is_standby):
if is_standby:
self._status.blink(True)
self._motors.disable()
self._switch.off()
self._infrared_trio.disable()
self._standby = True
else:
self._status.blink(False)
self._motors.enable()
self._switch.on()
self._infrared_trio.enable()
self._status.enable()
self._standby = False
# ..........................................................................
def get_current_message(self):
return self._current_message
# ..........................................................................
def _clear_current_message(self):
self._current_message = None
self._log.info('clear current message {}.'.format(
self._current_message))
# ..........................................................................
def act(self, message, callback):
'''
Responds to the Event contained within the Message.
'''
self._current_message = message
_port_speed = self._motors.get_current_power_level(Orientation.PORT)
_stbd_speed = self._motors.get_current_power_level(Orientation.STBD)
if _port_speed is not None and _stbd_speed is not None:
self._log.debug(
'last set power port: {:6.1f}, starboard: {:6.1f}'.format(
_port_speed, _stbd_speed))
else:
self._log.debug('last set power port: {}, starboard: {}'.format(
_port_speed, _stbd_speed))
event = self._current_message.get_event()
self._current_message.start()
# BUTTON ..............................................
# button ...............................................................
if event is Event.BUTTON:
# if button pressed we change standby mode but don't do anything else
_value = self._current_message.get_value()
# toggle value
# self.set_standby(not _value)
if _value:
self._log.critical('event: button value: {}.'.format(_value))
self.set_standby(False)
else:
self._log.error('event: button value: {}.'.format(_value))
self.set_standby(True)
# if in standby mode we don't process the event, but we do perform the callback
if self._standby:
self._log.info('event: disabled, currently in standby mode.')
callback(self._current_message,
self._motors.get_current_power_levels())
# ballistic behaviours ...........................................................
# BATTERY_LOW ..............................................
elif event is Event.BATTERY_LOW:
if self._enable_self_shutdown:
self._log.critical('event: battery low.')
self.set_standby(True)
self._current_message.complete()
self._callback_shutdown()
else:
self._log.warning(
'event ignored: battery low (self shutdown disabled).')
self._current_message.complete()
# callback(self._current_message, None)
# self._clear_current_message()
# return
# SHUTDOWN ..............................................
# shutdown .............................................................
elif event is Event.SHUTDOWN:
if self._enable_self_shutdown:
self._log.info('event: shutdown.')
self.set_standby(True)
self._callback_shutdown()
else:
self._log.warning(
'event ignored: shutdown (self shutdown disabled).')
# self._current_message.complete()
# callback(self._current_message, None)
# self._clear_current_message()
# return
# # stopping and halting ...................................................
# STOP ..............................................
# stop .................................................................
elif event is Event.STOP:
self._log.info('event: stop.')
self._motors.stop()
# HALT ..............................................
# halt .................................................................
elif event is Event.HALT:
self._log.info('event: halt.')
self._motors.halt()
# BRAKE ...................................
# brake ................................................................
elif event is Event.BRAKE:
self._log.info('event: brake.')
self._motors.brake()
# STANDBY ..............................................
# standby ..............................................................
elif event is Event.STANDBY:
self._log.info('event: standby.')
# disable motors until button press
self._motors.disable()
self._switch.off()
self._standby = True
# # bumper .............................................................
# BUMPER_PORT ..............................................
elif event is Event.BUMPER_PORT:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._infrared_trio.disable()
self._motors.stop()
# self._info.blink_side(Orientation.PORT, 1)
# self._motors.astern(Speed.HALF.value)
self._motors.turnAstern(Speed.THREE_QUARTER.value,
Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._infrared_trio.enable()
self._log.info('action complete: port bumper.')
else:
self._log.info('no action required (not moving): port bumper.')
# BUMPER_CENTER ..............................................
elif event is Event.BUMPER_CENTER:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._infrared_trio.disable()
self._motors.stop()
# self._info.blink(1)
self._motors.astern(Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._infrared_trio.enable()
self._log.info('action complete: center bumper.')
else:
self._log.info(
'no action required (not moving): center bumper.')
# BUMPER_STBD ..............................................
elif event is Event.BUMPER_STBD:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._motors.stop()
# self._info.blink_side(Orientation.STBD, 1)
# self._motors.astern(Speed.FULL.value)
self._motors.turnAstern(Speed.HALF.value,
Speed.THREE_QUARTER.value)
time.sleep(0.5)
self._motors.brake()
self._log.info('action complete: starboard bumper.')
else:
self._log.info(
'no action required (not moving): starboard bumper.')
# BUMPER_UPPER ..............................................
elif event is Event.BUMPER_UPPER:
self._log.info(Fore.RED + Style.BRIGHT + 'event: emergency stop.')
self._motors.stop()
# # infrared ...........................................................
# INFRARED_PORT ..............................................
elif event is Event.INFRARED_PORT:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port infrared.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._infrared_trio.disable()
self._motors.halt()
# self._info.blink_side(Orientation.PORT, 1)
# self._motors.astern(Speed.HALF.value)
self._motors.turnAstern(Speed.THREE_QUARTER.value,
Speed.HALF.value)
time.sleep(0.1)
self._motors.brake()
self._infrared_trio.enable()
self._log.info('action complete: port infrared.')
else:
self._log.info(
'no action required (not moving): port infrared.')
# INFRARED_CENTER ..............................................
elif event is Event.INFRARED_CENTER:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center infrared.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._infrared_trio.disable()
self._motors.halt()
# self._info.blink(1)
self._motors.astern(Speed.HALF.value)
time.sleep(0.2)
self._motors.brake()
self._infrared_trio.enable()
self._log.info('action complete: center infrared.')
else:
self._log.info(
'no action required (not moving): center infrared.')
# INFRARED_SHORT_RANGE .........................................
elif event is Event.INFRARED_SHORT_RANGE:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' short range infrared.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._infrared_trio.disable()
self._motors.halt()
# self._info.blink(1)
self._motors.astern(Speed.HALF.value)
time.sleep(0.34)
self._motors.brake()
self._infrared_trio.enable()
self._log.info('action complete: short range infrared.')
else:
self._log.info(
'no action required (not moving): short range infrared.')
self._infrared_trio.set_long_range_scan(True)
# INFRARED_LONG_RANGE .........................................
elif event is Event.INFRARED_LONG_RANGE:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' long range infrared.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._log.critical(
"WE'RE GOING TOO FAST! xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx "
)
self._infrared_trio.set_long_range_scan(False)
self._motors.change_speed(Speed.DEAD_SLOW.value)
# self._motors.ahead(Speed.DEAD_SLOW.value)
self._log.info('action complete: long range infrared.')
# if self._motors.is_faster_than(Speed.SLOW) or True: # if we're moving fast then slow down
# self._log.critical('WE\'RE GOING TOO FAST!')
# self._motors.ahead(Speed.SLOW.value)
# self._log.info('action complete: long range infrared.')
else:
self._log.critical(
'no action required (not moving): long range infrared.')
# INFRARED_STBD ..............................................
elif event is Event.INFRARED_STBD:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard infrared.' +
Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._motors.halt()
# self._info.blink_side(Orientation.STBD, 1)
# self._motors.astern(Speed.FULL.value)
self._motors.turnAstern(Speed.HALF.value,
Speed.THREE_QUARTER.value)
time.sleep(0.1)
self._motors.brake()
self._log.info('action complete: starboard infrared.')
else:
self._log.info(
'no action required (not moving): starboard infrared.')
# # emergency movements ................................................
# EMERGENCY_ASTERN ..............................................
# # movement ahead .....................................................
# FULL_AHEAD ..............................................
# HALF_AHEAD ..............................................
# SLOW_AHEAD ..............................................
# DEAD_SLOW_AHEAD ..............................................
# AHEAD ..............................................
# 8 ahead ..............................................................
elif event is Event.AHEAD:
if self._infrared_trio.get_long_range_hits() > 0:
self._log.info('event: ahead slow speed.')
_port_speed = Speed.SLOW.value
_stbd_speed = Speed.SLOW.value
else:
self._log.info('event: ahead half speed.')
_port_speed = Speed.HALF.value
_stbd_speed = Speed.HALF.value
self._motors.ahead(Speed.HALF.value)
self._log.info('action complete: ahead.')
# # movement astern ....................................................
# ASTERN ..............................................
# 2 astern .............................................................
elif event is Event.ASTERN:
# _port_speed = -1.0 * Speed.HALF.value
# _stbd_speed = -1.0 * Speed.HALF.value
self._log.critical('event: astern.')
self._infrared_trio.disable()
self._motors.astern(Speed.HALF.value)
self._infrared_trio.enable()
self._log.critical('event (returned already): astern.')
# DEAD_SLOW_ASTERN ..............................................
# SLOW_ASTERN ..............................................
# HALF_ASTERN ..............................................
# FULL_ASTERN ..............................................
# # relative change ....................................................
# INCREASE_SPEED ..............................................
# EVEN ..............................................
# DECREASE_SPEED ..............................................
# # port turns .........................................................
# TURN_AHEAD_PORT ..............................................
# 9 turn ahead port ....................................................
elif event is Event.TURN_AHEAD_PORT:
self._log.critical('event: turn ahead to port.')
_stbd_speed += step_value
_stbd_speed = min(Speed.MAXIMUM.value, _stbd_speed)
self._log.info(
'event: turn ahead to port: {:6.1f}, {:6.1f}'.format(
_port_speed, _stbd_speed))
self._motors.turnAhead(_port_speed, _stbd_speed)
# TURN_TO_PORT ..............................................
# TURN_ASTERN_PORT ..............................................
# SPIN_PORT ..............................................
# 4 spin counter-clockwise .............................................
elif event is Event.SPIN_PORT:
self._log.info('event: spinPort.')
self._motors.spinPort(Speed.THREE_QUARTER.value)
# # starboard turns ....................................................
# SPIN_STBD ..............................................
# 6 spin clockwise .....................................................
elif event is Event.SPIN_STBD:
self._log.info('event: spinStarboard.')
self._motors.spinStarboard(Speed.THREE_QUARTER.value)
# TURN_ASTERN_STBD ..............................................
# TURN_TO_STBD ..............................................
# TURN_AHEAD_STBD ..............................................
# 7 turn ahead starboard ...............................................
elif event is Event.TURN_AHEAD_STBD:
self._log.critical('event: turn ahead to starboard.')
_port_speed += step_value
_port_speed = min(Speed.MAXIMUM.value, _port_speed)
self._log.info('Turn ahead to starboard: {:6.1f}, {:6.1f}'.format(
_port_speed, _stbd_speed))
self._motors.turnAhead(_port_speed, _stbd_speed)
# ROAM ..............................................
elif event is Event.ROAM:
self._log.critical('event: roam.')
self._lidar.enable()
self._rgbmatrix.disable()
self._rgbmatrix.clear()
values = self._lidar.scan()
# time.sleep(2.0)
# self._infrared_trio.disable()
# self._motors.spinPort(Speed.THREE_QUARTER.value)
# time.sleep(0.20)
# self._motors.halt()
# self._motors.ahead(Speed.HALF.value)
# self._infrared_trio.enable()
# time.sleep(2.3)
# self._motors.spinStarboard(Speed.THREE_QUARTER.value)
# time.sleep(0.4)
# self._motors.ahead(Speed.HALF.value)
# time.sleep(1.4)
# self._motors.halt()
# self._motors.spinPort(Speed.THREE_QUARTER.value)
# time.sleep(2.0)
# self._motors.halt()
# for i in range(5):
# self._rgbmatrix.set_color(Color.GREEN)
# time.sleep(0.5)
# self._rgbmatrix.set_color(Color.BLACK)
# time.sleep(0.5)
# self._rgbmatrix.set_color(Color.BLACK)
self._log.critical('scan complete.')
# non-ballistic behaviours .......................................................
else:
self._log.error('unrecognised event: {}'.format(event))
pass
callback(self._current_message,
self._motors.get_current_power_levels())
self._clear_current_message()
# of the Robot Operating System project, released under the MIT License. Please
# see the LICENSE file included as part of this package.
#
from colorama import init, Fore, Style
init()
from lib.logger import Logger, Level
_log = Logger("color-test", Level.DEBUG)
_log.debug('debug.')
_log.info('info.')
_log.notice('notice.')
_log.warning('warning.')
_log.error('error.')
_log.critical('critical.')
_log.heading('title', 'message.', 'info [0/0]')
_log.info(Fore.RED + 'RED')
_log.info(Fore.GREEN + 'GREEN')
_log.info(Fore.BLUE + 'BLUE')
_log.info(Fore.YELLOW + 'YELLOW')
_log.info(Fore.MAGENTA + 'MAGENTA')
_log.info(Fore.CYAN + 'CYAN')
_log.info(Fore.BLACK + 'BLACK')
_log.info(Fore.WHITE + 'WHITE')
