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')