class BNO055: ''' Reads from a BNO055 9DoF sensor. In order for this to be used by a Raspberry Pi you must in theory slow your I2C bus down to around 10kHz, though if not things still seem to work. YMMV. Note that this has been calibrated based on the orientation of the BNO055 board as installed on the KR01 robot, with the dot on the chip (mounted horizontally) as follows: Fore -------------- | • | | | | | Port | | Starboard | | -------------- Aft If you mount the chip in a different orientation you will likely need to multiply one or more of the axis by -1.0. ''' def __init__(self, config, queue, level): self._log = Logger("bno055", level) if config is None: raise ValueError("no configuration provided.") self._queue = queue self._config = config # config _config = self._config['ros'].get('bno055') self._loop_delay_sec = _config.get('loop_delay_sec') _i2c_device = _config.get('i2c_device') self._pitch_trim = _config.get('pitch_trim') self._roll_trim = _config.get('roll_trim') self._heading_trim = _config.get('heading_trim') self._error_range = 5.0 # permitted error between Euler and Quaternion (in degrees) to allow setting value, was 3.0 self._rate = Rate(_config.get('sample_rate')) # e.g., 20Hz # use `ls /dev/i2c*` to find out what i2c devices are connected self._bno055 = adafruit_bno055.BNO055_I2C( I2C(_i2c_device)) # Device is /dev/i2c-1 # some of the other modes provide accurate compass calibration but are very slow to calibrate # self._bno055.mode = BNO055Mode.COMPASS_MODE.mode # self._bno055.mode = BNO055Mode.NDOF_FMC_OFF_MODE.mode self._counter = itertools.count() self._thread = None self._enabled = False self._closed = False self._heading = 0.0 # we jauntily default at zero so we don't return a None self._pitch = None self._roll = None self._is_calibrated = Calibration.NEVER self._log.info('ready.') # .......................................................................... def enable(self): if not self._closed: self._enabled = True # if we haven't started the thread yet, do so now... if self._thread is None: self._thread = threading.Thread(target=BNO055._read, args=[self]) self._thread.start() self._log.info('enabled.') else: self._log.warning('thread already exists.') else: self._log.warning('cannot enable: already closed.') # .......................................................................... def get_heading(self): ''' Returns a tuple containing a Calibration (enum) indicating if the sensor is calibrated followed by the heading value. ''' return (self._is_calibrated, self._heading) # .......................................................................... def get_pitch(self): return self._pitch # .......................................................................... def get_roll(self): return self._roll # .......................................................................... def is_calibrated(self): return self._is_calibrated # .......................................................................... def _read(self): ''' The function that reads sensor values in a loop. This checks to see if the 'sys' calibration is at least 3 (True), and if the Euler and Quaternion values are within an error range of each other, this sets the class variable for heading, pitch and roll. If they aren't within range for more than n polls, the values revert to None. ''' self._log.info('starting sensor read...') _e_heading, _e_pitch, _e_roll, _e_yaw, _q_heading, _q_pitch, _q_roll, _q_yaw = [ None ] * 8 _quat_w = 0 _quat_x = 0 _quat_y = 0 _quat_z = 0 _count = 0 # begin loop ....................................... while self._enabled and not self._closed: _status = self._bno055.calibration_status # status: sys, gyro, accel, mag _sys_calibrated = (_status[0] >= 3) _gyr_calibrated = (_status[1] >= 3) _acc_calibrated = (_status[2] >= 2) _mag_calibrated = (_status[3] >= 2) # "Therefore we recommend that as long as Magnetometer is 3/3, and Gyroscope is 3/3, the data can be trusted." # source: https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BNO055-Calibration-Staus-not-stable/td-p/8375 _ok_calibrated = _gyr_calibrated and _mag_calibrated # show calibration of each sensor self._log.info( Fore.MAGENTA + ( Style.BRIGHT if _sys_calibrated else Style.DIM ) + ' s{}'.format(_status[0]) \ + Fore.YELLOW + ( Style.BRIGHT if _gyr_calibrated else Style.DIM ) + ' g{}'.format(_status[1]) \ + Fore.GREEN + ( Style.BRIGHT if _acc_calibrated else Style.DIM ) + ' a{}'.format(_status[2]) \ + Fore.CYAN + ( Style.BRIGHT if _mag_calibrated else Style.DIM ) + ' m{}'.format(_status[3]) ) # BNO055 Absolute Orientation (Quaterion, 100Hz) Four point quaternion output for more accurate data manipulation .............. _quat = self._bno055.quaternion if _quat != None: _quat_w = _quat[0] if _quat[0] != None else None _quat_x = _quat[1] if _quat[1] != None else None _quat_y = _quat[2] if _quat[2] != None else None _quat_z = _quat[3] if _quat[3] != None else None if _quat_w != None \ and _quat_x != None \ and _quat_y != None \ and _quat_z != None: _q = Quaternion(_quat_w, _quat_x, _quat_y, _quat_z) _q_heading = _q.degrees _q_yaw_pitch_roll = _q.yaw_pitch_roll _q_yaw = _q_yaw_pitch_roll[0] _q_pitch = _q_yaw_pitch_roll[1] _q_roll = _q_yaw_pitch_roll[2] time.sleep(0.5) # TEMP # heading ............................................................ self._log.debug( Fore.BLUE + Style.NORMAL + '_quat_w={:>5.4f}, _quat_x={:>5.4f}, _quat_y={:>5.4f}, _quat_z={:>5.4f}' .format(_quat_w, _quat_x, _quat_y, _quat_z)) if _ok_calibrated: self._heading = _q_heading + self._heading_trim self._log.info(Fore.MAGENTA + 'heading={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw) \ + Fore.CYAN + Style.DIM + '\tquat: {}'.format(_quat)) self._is_calibrated = Calibration.CALIBRATED else: self._log.info(Fore.CYAN + Style.DIM + 'heading={:>5.4f}\t p={:>5.4f}\t r={:>5.4f}\t y={:>5.4f}'.format(_q_heading, _q_pitch, _q_roll, _q_yaw) \ + Fore.CYAN + Style.DIM + '\tquat: {}'.format(_quat)) self._is_calibrated = Calibration.LOST # # # pitch .............................................................. # # if Convert.in_range(_e_pitch, _q_pitch, self._error_range): # # self._pitch = _e_pitch + self._pitch_trim # # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (calibrated)'.format(_e_pitch)) # # else: # # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'pitch: \t{:>5.4f}° (euler)'.format(_e_pitch)) # # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'pitch: \t{:>5.4f}° (quaternion)'.format(_q_pitch)) # # self._pitch = None # # # roll ............................................................... # # if Convert.in_range(_e_roll, _q_roll, self._error_range): # # self._roll = _e_roll + self._roll_trim # # self._log.debug(' ' + Fore.CYAN + Style.BRIGHT + 'roll: \t{:>5.4f}° (calibrated)'.format(_e_roll)) # # else: # # self._log.debug(' ' + Fore.YELLOW + Style.BRIGHT + 'roll: \t{:>5.4f}° (euler)'.format(_e_roll)) # # self._log.debug(' ' + Fore.GREEN + Style.BRIGHT + 'roll: \t{:>5.4f}° (quaternion)'.format(_q_roll)) # # self._roll = None else: pass _count = next(self._counter) self._rate.wait() # end loop ..................................... self._log.debug('read loop ended.') # .......................................................................... def disable(self): if self._enabled: self._enabled = False self._log.info('disabled.') else: self._log.warning('already disabled.') # .......................................................................... def close(self): if not self._closed: if self._enabled: self.disable() self._closed = True self._log.info('closed.') else: self._log.warning('already closed.')
def test_motors(): _log = Logger('test', Level.INFO) # read YAML configuration _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) # rotary controller _min = 0 _max = 50 _step = 5 _rotary_ctrl = RotaryControl(_config, _min, _max, _step, Level.WARN) _motor_configurer = MotorConfigurer(_config, Level.INFO) _motors = _motor_configurer.configure() _motors.enable() _log.info('starting motors...') _port_motor = _motors.get_motor(Orientation.PORT) _port_pid = PIDController(_config, _port_motor, level=Level.INFO) if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT: _port_motor.enable() _port_pid.enable() _stbd_motor = _motors.get_motor(Orientation.STBD) _stbd_pid = PIDController(_config, _stbd_motor, level=Level.INFO) _stbd_motor.enable() _stbd_pid.enable() _power = 0.0 _pot = Potentiometer(_config, Level.WARN) if POT_CONTROL: _pot_limit = 1.0 else: _pot_limit = 0.0 _pot.set_output_limits(0.0, _pot_limit) try: _rate = Rate(10) i = 0 # for i in range(7): while True: _rot_value = _rotary_ctrl.read() # _unscaled_value = _pot.get_value() # _pot.set_rgb(_unscaled_value) _scaled_value = _pot.get_scaled_value(True) _log.info('rotary : {:<5.2f};'.format(_rot_value) + ' pot: {:<5.2f}'.format(_scaled_value)) if POT_CONTROL: _power = _scaled_value # _power = i * 0.1 _log.info(Fore.YELLOW + 'set motor power: {:5.2f} (original: {:d});'.format( _power, i)) if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT: _port_motor.set_motor_power(-1.0 * _power) _stbd_motor.set_motor_power(_power) _log.info('[{:d}]\t'.format(i) \ + Fore.RED + 'power {:5.2f}/{:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f};\t'.format(\ _stbd_motor.get_current_power_level(), _power, _port_pid.steps, _port_pid.setpoint, _port_pid.velocity) \ + Fore.GREEN + 'power {:5.2f}/{:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f}'.format(\ _port_motor.get_current_power_level(), _power, _stbd_pid.steps, _stbd_pid.setpoint, _stbd_pid.velocity)) else: if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT: _port_pid.setpoint = _rot_value _stbd_pid.setpoint = _rot_value # _log.info(Fore.RED + 'STBD value: {:5.2f} setpoint: {:5.2f}'.format(_scaled_value, _stbd_pid.setpoint) + Style.RESET_ALL) _log.info('[{:d}]\t'.format(i) \ + Fore.RED + 'motor power {:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f};\t'.format(\ _stbd_motor.get_current_power_level(), _port_pid.steps, _port_pid.setpoint, _port_pid.velocity) \ + Fore.GREEN + 'motor power {:5.2f}; {:>4d} steps; setpoint: {:5.2f}; velocity: {:5.2f}'.format(\ _port_motor.get_current_power_level(), _stbd_pid.steps, _stbd_pid.setpoint, _stbd_pid.velocity)) # if i > 2 and ( _port_motor.steps == 0 or _stbd_motor.steps == 0 ): # raise Exception('motors are not turning.') # time.sleep(1.0) # time.sleep(1.0) _rate.wait() # tests... if ORIENTATION == Orientation.BOTH or ORIENTATION == Orientation.PORT: _log.info('port motor: {:d} steps.'.format(_port_pid.steps)) # assert _port_motor.steps > 0 _log.info('stbd motor: {:d} steps.'.format(_stbd_pid.steps)) # assert _stbd_motor.steps > 0 except Exception as e: _log.error('error: {}'.format(e)) finally: close_motors(_log, _port_motor, _stbd_motor) _log.info(Fore.YELLOW + 'complete.')
class PID(): ''' A PID controller seeks to keep some input variable close to a desired setpoint by adjusting an output. The way in which it does this can be 'tuned' by adjusting three parameters (P,I,D). This uses the ros:motors:pid: and ros:geometry: sections of the YAML configuration. If a FileWriter is set odometry data will be written to a data file. Various formulae are found at the bottom of this file. ''' def __init__(self, config, motor, level): self._motor = motor self._orientation = motor.get_orientation() self._log = Logger('pid:{}'.format(self._orientation.label), level) # PID configuration ................................ if config is None: raise ValueError('null configuration argument.') _config = config['ros'].get('motors').get('pid') self._enable_slew = _config.get('enable_slew') if self._enable_slew: self._slewlimiter = SlewLimiter(config, self._orientation, level) # geometry configuration ........................... _geo_config = config['ros'].get('geometry') self._wheel_diameter = _geo_config.get('wheel_diameter') self._wheelbase = _geo_config.get('wheelbase') self._step_per_rotation = _geo_config.get('steps_per_rotation') _clip_max = _config.get('clip_limit') _clip_min = -1.0 * _clip_max self._clip = lambda n: _clip_min if n <= _clip_min else _clip_max if n >= _clip_max else n self._enable_p = _config.get('enable_p') self._enable_i = _config.get('enable_i') self._enable_d = _config.get('enable_d') _kp = _config.get('kp') _ki = _config.get('ki') _kd = _config.get('kd') self.set_tuning(_kp, _ki, _kd) self._read_p_from_pot = _config.get('read_p_from_pot') self._read_i_from_pot = _config.get('read_i_from_pot') self._read_d_from_pot = _config.get('read_d_from_pot') if self._read_p_from_pot or self._read_i_from_pot or self._read_d_from_pot: self._pot = Potentiometer(config, level) # set scaled output maximum value to double k* if self._read_p_from_pot: _out_max = 0.01500 self._log.info(Fore.YELLOW + 'scaled output maximum set from KP: {:9.6f}'. format(_out_max)) self._pot.set_out_max(_out_max) elif self._read_i_from_pot: _out_max = 0.000470 self._log.info(Fore.YELLOW + 'scaled output maximum set from KI: {:9.6f}'. format(_out_max)) self._pot.set_out_max(_out_max) elif self._read_d_from_pot: _out_max = 0.00200 self._log.info(Fore.YELLOW + 'scaled output maximum set from KD: {:9.6f}'. format(_out_max)) self._pot.set_out_max(_out_max) self._rate = Rate(_config.get('sample_freq_hz'), level) # default sample rate is 20Hz # self._millis = lambda: int(round(time.time() * 1000)) self._counter = itertools.count() self._last_error = 0.0 self._sum_errors = 0.0 self._step_limit = 0 self._stats_queue = None self._filewriter = None # optional: for statistics self._filewriter_closed = False _wheel_circumference = math.pi * self._wheel_diameter # mm self._steps_per_m = 1000.0 * self._step_per_rotation / _wheel_circumference self._log.info(Fore.BLUE + Style.BRIGHT + '{:5.2f} steps per m.'.format(self._steps_per_m)) self._steps_per_cm = 10.0 * self._step_per_rotation / _wheel_circumference self._log.info(Fore.BLUE + Style.BRIGHT + '{:5.2f} steps per cm.'.format(self._steps_per_cm)) self._last_steps = 0 self._max_diff_steps = 0 self._enabled = False self._closed = False self._thread = None self._log.info('ready.') # .......................................................................... def set_tuning(self, p, i, d): ''' Set the tuning of the power PID controller. ''' self._kp = p self._ki = i self._kd = d self._log.info( 'set PID tuning for {} motor: P={:>8.5f}; I={:>8.5f}; D={:>8.5f}.'. format(self._orientation.name, self._kp, self._ki, self._kd)) # .......................................................................... def get_tuning(self): return [self._kp, self._ki, self._kd] # .......................................................................... def get_tuning_info(self): ''' Returns the tuning information, formatted and including if the setting is enabled or disabled. ''' _en_p = 'on' if self._enable_p else 'off' _en_i = 'on' if self._enable_i else 'off' _en_d = 'on' if self._enable_d else 'off' return [ '{:d}'.format(self._elapsed_sec),\ '{:6.4f} ({})'.format(self._kp, _en_p),\ '{:6.4f} ({})'.format(self._ki, _en_i),\ '{:6.4f} ({})'.format(self._kd, _en_d) ] # .............................................................................. def get_steps(self): ''' A convenience method that returns the number of steps registered by the motor encoder. ''' return self._motor.get_steps() # .............................................................................. def reset_steps(self): ''' A convenience method that resets the step count to zero. ''' self._motor.reset_steps() # .......................................................................... def is_stepping(self, direction): ''' Returns True if the motor step count is less than the currently-set step limit, or if traveling in REVERSE, greater than the step limit. ''' # while f_is_enabled() and (( direction is Direction.FORWARD and self.get_steps() < _step_limit ) or \ # ( direction is Direction.REVERSE and self.get_steps() > _step_limit )): # return ( self.get_steps() < self._step_limit ) if direction is Direction.FORWARD else ( self.get_steps() > self._step_limit ) if direction is Direction.FORWARD: self._log.debug( Fore.MAGENTA + '{} is stepping forward? {:>5.2f} < {:>5.2f}'.format( self._orientation, self.get_steps(), self._step_limit)) return (self.get_steps() < self._step_limit) elif direction is Direction.REVERSE: self._log.debug( Fore.MAGENTA + '{} is stepping reverse? {:>5.2f} > {:>5.2f}'.format( self._orientation, self.get_steps(), self._step_limit)) return (self.get_steps() > self._step_limit) else: raise Exception('no direction provided.') # .......................................................................... def set_step_limit(self, direction, steps): ''' Sets the step limit (i.e., the goal) prior to a call to step_to(). ''' if direction is Direction.FORWARD: self._step_limit = self.get_steps() + steps elif direction is Direction.REVERSE: self._step_limit = self.get_steps() - steps else: raise Exception('no direction provided.') # .......................................................................... def set_velocity(self, velocity): ''' Sets the velocity of the motor to the specified Velocity. ''' if type(velocity) is Velocity: self._target_velocity = velocity.value elif isinstance(velocity, float): self._target_velocity = velocity else: raise Exception('expected Velocity argument, not {}'.format( type(velocity))) # .......................................................................... def reset_max_diff_steps(self): self._max_diff_steps = 0 # .......................................................................... def get_max_diff_steps(self): return self._max_diff_steps # .......................................................................... def _loop(self): ''' The PID loop that continues until the enabled flag is false. ''' while self._enabled: if self._read_p_from_pot: self._kp = self._pot.get_scaled_value() self._log.info( Fore.BLUE + 'P={:>10.7f};'.format(self._kp) + Fore.BLACK + ' I={:>10.7f};'.format(self._ki) + Fore.BLACK + ' D={:>10.7f};'.format(self._kd) + Fore.GREEN + Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format( self._motor.get_velocity(), self._target_velocity)) elif self._read_i_from_pot: self._ki = self._pot.get_scaled_value() self._log.info( Fore.BLACK + 'P={:>10.7f};'.format(self._kp) + Fore.BLUE + ' I={:>10.7f};'.format(self._ki) + Fore.BLACK + ' D={:>10.7f};'.format(self._kd) + Fore.GREEN + Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format( self._motor.get_velocity(), self._target_velocity)) elif self._read_d_from_pot: self._kd = self._pot.get_scaled_value() self._log.info( Fore.BLACK + 'P={:>10.7f};'.format(self._kp) + Fore.BLACK + ' I={:>10.7f};'.format(self._ki) + Fore.BLUE + ' D={:>10.7f};'.format(self._kd) + Fore.GREEN + Style.BRIGHT + ' velocity: {:>5.2f}/{:>5.2f}'.format( self._motor.get_velocity(), self._target_velocity)) else: self._log.info(Fore.BLACK + 'P={:>10.7f}; I={:>10.7f}; D={:>10.7f}'.format( self._kp, self._ki, self._kd)) _this_steps = self.get_steps() _diff_steps = _this_steps - self._last_steps self._max_diff_steps = max(self._max_diff_steps, _diff_steps) # time.sleep(0.04) self._log.debug('PID loop; diff steps: {:d}'.format(_diff_steps)) # TODO figure out how many ticks per loop is required for a given velocity # self._log.info(Fore.GREEN + Style.BRIGHT + 'current velocity: {};'.format(self._motor.get_velocity()) \ # + Fore.CYAN + Style.NORMAL + ' target velocity: {}; step limit: {}.'.format(self._target_velocity, self._step_limit)) _changed = self._compute(self._target_velocity) self._last_steps = _this_steps self._rate.wait() # 20Hz self._log.info(Fore.MAGENTA + Style.BRIGHT + 'exited PID loop.') # .......................................................................... def step_to(self, target_velocity, direction, slew_rate, f_is_enabled): ''' Performs the PID calculation during a loop, returning once the number of steps have been reached. Velocity is the target velocity of the robot, expressed as either an enumerated value or a decimal from 0.0 to 100.0, where 50.0 is considered half-speed (though this will not be true in practice, until we tune these values). ''' if type(target_velocity) is not Velocity: raise Exception('expected Velocity argument, not {}'.format( type(target_velocity))) _target_velocity = target_velocity.value self._start_time = time.time() if self._enable_slew: if not self._slewlimiter.is_enabled(): print(Fore.GREEN + 'slew enabled.' + Style.RESET_ALL) self._slewlimiter.set_rate_limit(slew_rate) self._slewlimiter.enable() self._slewlimiter.start() else: print(Fore.GREEN + 'slew disabled; f_is_enabled? {}'.format(f_is_enabled()) + Style.RESET_ALL) if direction is Direction.FORWARD: # ....................................................... _is_accelerating = self._motor.get_velocity() < _target_velocity self._log.info(Fore.GREEN + Style.BRIGHT + 'current velocity: {};'.format(self._motor.get_velocity()) \ + Fore.CYAN + Style.NORMAL + ' target velocity: {}; step limit: {}; is_accelerating: {}.'.format(_target_velocity, self._step_limit, _is_accelerating)) while f_is_enabled() and self.is_stepping( direction ): # or ( direction is Direction.REVERSE and self.get_steps() > self._step_limit )): if self._enable_slew: _slewed_target_velocity = self._slewlimiter.slew( self._motor.get_velocity(), _target_velocity) _changed = self._compute(_slewed_target_velocity) else: _changed = self._compute(_target_velocity) self._log.debug(Fore.BLACK + Style.DIM + '{:d}/{:d} steps.'.format( self.get_steps(), self._step_limit)) if not _changed and self._motor.get_velocity( ) == 0.0 and _target_velocity == 0.0 and self._step_limit > 0: # we will never get there if we aren't moving self._log.info(Fore.RED + 'break 2.') break # displays info only: if _is_accelerating and self._motor.get_velocity( ) >= _target_velocity: _elapsed = time.time() - self._start_time _is_accelerating = False self._log.info( Fore.GREEN + Style.BRIGHT + 'reached target velocity of {:+06.2f} at {:5.2f} sec elapsed.' .format(_target_velocity, _elapsed)) self._rate.wait() # 20Hz if self._motor.is_interrupted(): self._log.info(Fore.RED + 'motor interrupted.') break if self._orientation is Orientation.PORT: self._log.info(Fore.RED + 'current velocity: {:>5.2f};'.format(self._motor.get_velocity() ) \ + Fore.CYAN + ' target velocity: {}; {:d} steps of: {}; is_accelerating: {}.'.format(_target_velocity, self.get_steps(), self._step_limit, _is_accelerating)) else: self._log.info(Fore.GREEN + 'current velocity: {:>5.2f};'.format(self._motor.get_velocity() ) \ + Fore.CYAN + ' target velocity: {}; {:d} steps of: {}; is_accelerating: {}.'.format(_target_velocity, self.get_steps(), self._step_limit, _is_accelerating)) elif direction is Direction.REVERSE: # ..................................................... _target_velocity = -1.0 * _target_velocity _is_accelerating = self._motor.get_velocity() < _target_velocity self._log.info( Fore.RED + Style.BRIGHT + 'target velocity: {}; step limit: {}; is_accelerating: {}.'. format(_target_velocity, self._step_limit, _is_accelerating)) while f_is_enabled() and self.is_stepping( direction ): # or ( direction is Direction.REVERSE and self.get_steps() > self._step_limit )): if self._enable_slew: _slewed_target_velocity = self._slewlimiter.slew( self._motor.get_velocity(), _target_velocity) _changed = self._compute(_slewed_target_velocity) else: _changed = self._compute(_target_velocity) self._log.debug(Fore.BLACK + Style.DIM + '{:d}/{:d} steps.'.format( self.get_steps(), self._step_limit)) if not _changed and self._motor.get_velocity( ) == 0.0 and _target_velocity == 0.0 and self._step_limit > 0: # we will never get there if we aren't moving break # displays info only: if _is_accelerating and self._motor.get_velocity( ) >= _target_velocity: _elapsed = time.time() - self._start_time _is_accelerating = False self._log.info( Fore.GREEN + Style.BRIGHT + 'reached target velocity of {:+06.2f} at {:5.2f} sec elapsed.' .format(_target_velocity, _elapsed)) self._rate.wait() # 20Hz if self._motor.is_interrupted(): break else: # .................................................................................... raise Exception('no direction provided.') _elapsed = time.time() - self._start_time self._elapsed_sec = int(round(_elapsed)) + 1 self._log.info( '{} motor step-to complete; {:5.2f} sec elapsed;'.format( self._orientation.name, _elapsed) + Fore.MAGENTA + ' {:d}/{:d} steps.'.format(self.get_steps(), self._step_limit)) if self._enable_slew: # if not a repeat call self._slewlimiter.reset(0.0) self._motor.reset_interrupt() # .......................................................................... def _compute(self, target_velocity): ''' The PID power compute function, called upon each loop. Returns True if PID computed a change, False if the error was zero and no change was applied. ''' self._loop_count = next(self._counter) # compare current and target velocities ............................ _current_velocity = self._motor.get_velocity() _error = target_velocity - _current_velocity # if _current_velocity == 0: # self._log.debug(Fore.CYAN + Style.NORMAL + '[{:0d}]; velocity: {:+06.2f} ➔ {:+06.2f}\t _error: {:>5.2f}.'.format(self._loop_count, _current_velocity, target_velocity, _error)) # else: # self._log.debug(Fore.CYAN + Style.BRIGHT + '[{:0d}]; velocity: {:+06.2f} ➔ {:+06.2f}\t _error: {:>5.2f}.'.format(self._loop_count, _current_velocity, target_velocity, _error)) _current_power = self._motor.get_current_power_level() * ( 1.0 / self._motor.get_max_power_ratio()) _power_level = _current_power if PID.equals_zero(_error): if target_velocity == 0.0: self._motor.set_motor_power(0.0) self._log.info( Fore.WHITE + Style.BRIGHT + '[{:02d}]+ NO ERROR, currently at zero velocity.'.format( self._loop_count)) else: self._log.info(Fore.WHITE + Style.BRIGHT + '[{:02d}]+ no error, currently at target velocity: {:+06.2f}; at power: {:+5.3f}'.format(\ self._loop_count, _current_velocity, _current_power)) # remember some variables for next time ........................ self._last_error = 0.0 _changed = False else: # P: Proportional .............................................. if self._enable_p: _p_diff = self._kp * _error self._log.debug(Fore.BLUE + Style.BRIGHT + 'P diff: {:>5.4f}'.format(_p_diff) + Style.NORMAL + ' = self._kp: {:>5.4f} * _error: {:>5.4f}.'. format(self._kp, _error)) else: _p_diff = 0.0 # I: Integral .................................................. if self._enable_i: _i_diff = self._ki * self._sum_errors if self._enable_i else 0.0 self._log.debug(Fore.MAGENTA + Style.BRIGHT + 'I diff: {:45.4f}'.format(_i_diff) + Style.NORMAL \ + ' = self._ki: {:>5.4f} * _sum_errors: {:>5.4f}.'.format(self._ki, self._sum_errors)) else: _i_diff = 0.0 # D: Derivative ................................................ if self._enable_d: _d_diff = self._kd * self._last_error if self._enable_d else 0.0 self._log.debug( Fore.YELLOW + 'D diff: {:>5.4f}'.format(_d_diff) + Style.NORMAL + ' = self._kd: {:>5.4f} * _last_error: {:>5.4f}.'.format( self._kd, self._last_error)) else: _d_diff = 0.0 # calculate output ............................................. _output = _p_diff + _i_diff + _d_diff # self._log.debug(Fore.CYAN + Style.BRIGHT + '_output: {:>5.4f}'.format(_output) + Style.NORMAL + ' = (P={:+5.4f}) + (I={:+5.4f}) + (D={:+5.4f})'.format(_p_diff, _i_diff, _d_diff)) # clipping ..................................................... _clipped_output = self._clip(_output) # set motor power .............................................. _power_level = _current_power + _clipped_output # if abs(_output) <= 0.0005: # we're pretty close to the right value # self._log.debug(Fore.WHITE + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \ # + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\ # _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff)) # elif _output >= 0: # we're going too slow # self._log.debug(Fore.GREEN + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \ # + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\ # _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff)) # else: # we're going too fast # self._log.debug(Fore.RED + Style.NORMAL + '[{:02d}]+ velocity: {:+06.2f} ➔ {:+06.2f}'.format(self._loop_count, _current_velocity, target_velocity) \ # + Style.BRIGHT + '\t new power: {:+5.3f}'.format(_power_level) + Style.NORMAL + ' = current: {:+5.3f} + output: {:+5.3f} '.format(\ # _current_power, _clipped_output) + Style.DIM + '\tP={:+5.4f}\tI={:+5.4f}\tD={:+5.4f}'.format(_p_diff, _i_diff, _d_diff)) self._motor.set_motor_power(_power_level) # remember some variables for next time ........................ self._last_error = _error self._sum_errors += _error _changed = True # if configured, capture odometry data ................... if self._filewriter: _elapsed = time.time() - self._start_time _data = '{:>5.3f}\t{:>5.2f}\t{:>5.2f}\t{:>5.2f}\t{:d}\n'.format( _elapsed, (_power_level * 100.0), _current_velocity, target_velocity, self.get_steps()) self._stats_queue.append(_data) self._log.debug('odometry:' + Fore.BLACK + ' time: {:>5.3f};\tpower: {:>5.2f};\tvelocity: {:>5.2f}/{:>5.2f}\tsteps: {:d}'.format(\ _elapsed, _power_level, _current_velocity, target_velocity, self.get_steps())) return _changed # .......................................................................... def enable(self): if not self._closed: self._enabled = True # if we haven't started the thread yet, do so now... if self._thread is None: self._thread = threading.Thread(target=PID._loop, args=[self]) self._thread.start() self._log.info('PID loop for {} motor enabled.'.format( self._orientation)) else: self._log.warning( 'cannot enable PID loop for {} motor: thread already exists.' .format(self._orientation)) else: self._log.warning( 'cannot enable PID loop for {} motor: already closed.'.format( self._orientation)) # .......................................................................... def disable(self): self._enabled = False time.sleep(0.06) # just a bit more than 1 loop in 20Hz if self._thread is not None: self._thread.join(timeout=1.0) self._log.debug('pid thread joined.') self._thread = None self._log.info('PID loop for {} motor disabled.'.format( self._orientation)) # .......................................................................... def close(self): self.close_filewriter() self._log.info('closed.') # filewriter support ....................................................... # .............................................................................. def set_filewriter(self, filewriter): ''' If a FileWriter is set then the Motor will generate odometry data and write this to a data file whose directory and filename are determined by configuration. ''' self._filewriter = filewriter if self._filewriter: if self._stats_queue is None: self._stats_queue = deque() self._filewriter.enable(self._stats_queue) self._log.info('filewriter enabled.') # .......................................................................... def close_filewriter(self): if not self._filewriter_closed: self._filewriter_closed = True if self._filewriter: _tuning = self.get_tuning_info() self._filewriter.write_gnuplot_settings(_tuning) self._filewriter.disable() # formulae ................................................................. # .......................................................................... @staticmethod def equals_zero(value): ''' Returns True if the value is within 2% of 0.0. ''' return numpy.isclose(value, 0.0, rtol=0.02, atol=0.0) # .............................................................................. def get_distance_cm_for_steps(self, steps): ''' Provided a number of steps returns the distance traversed. ''' return steps / self._steps_per_cm # .............................................................................. def get_steps_for_distance_cm(self, distance_cm): ''' A function that returns the number of forward steps require to traverse the distance (measured in cm). Returns an int. ''' ''' Geometry Notes ................................... 494 encoder steps per rotation (maybe 493) 68.5mm diameter tires 215.19mm/21.2cm wheel circumference 1 wheel rotation = 215.2mm 2295 steps per meter 2295 steps per second = 1 m/sec 2295 steps per second = 100 cm/sec 229.5 steps per second = 10 cm/sec 22.95 steps per second = 1 cm/sec 1 rotation = 215mm = 494 steps 1 meter = 4.587 rotations 2295.6 steps per meter 22.95 steps per cm ''' return round(distance_cm * self._steps_per_cm) # .............................................................................. def get_forward_steps(self, rotations): ''' A function that return the number of steps corresponding to the number of wheel rotations. ''' return self._step_per_rotation * rotations
def main(): _log = Logger("vel-test", Level.INFO) _loader = ConfigLoader(Level.INFO) filename = 'config.yaml' _config = _loader.configure(filename) _motors = Motors(_config, None, Level.INFO) _port_pid = _motors.get_motor(Orientation.PORT).get_pid_controller() _stbd_pid = _motors.get_motor(Orientation.STBD).get_pid_controller() _pid = _stbd_pid Velocity.CONFIG.configure(_config) # sys.exit(0) try: _trial_time_sec = 10.0 # _motor_power = 0.99 _motor_velocity = 0.0 _target_velocity = 80.0 # _motors.get_motor(Orientation.PORT).get_thunderborg().SetMotor1(_motor_power) # direct to TB (max: 0.6?) # _motors.get_motor(Orientation.STBD).get_thunderborg().SetMotor2(_motor_power) # direct to TB (max: 0.6?) # _distance_cm = 100 # _fwd_steps_per_meter = _stbd_pid.get_steps_for_distance_cm(_distance_cm) # _log.info(Fore.MAGENTA + Style.NORMAL + 'calculated: {:>5.2f} steps per {:d} cm.'.format(_fwd_steps_per_meter, _distance_cm)) _current_value = 0 _limiter = SlewLimiter(_config, Orientation.STBD, Level.INFO) _limiter.enable() _rate = Rate(5) # Hz _pid.reset_steps() _pid.enable() # accelerate... # _motors.get_motor(Orientation.PORT).accelerate_to_velocity(_motor_velocity, SlewRate.NORMAL, -1) while _motor_velocity < _target_velocity: _motor_velocity = _limiter.slew(_motor_velocity, _target_velocity) _pid.set_velocity(_motor_velocity) _log.info(Fore.CYAN + Style.BRIGHT + 'velocity: {:>5.2f} -> {:>5.2f}.'.format( _motor_velocity, _target_velocity)) _rate.wait() # time.sleep(0.1) _log.info(Fore.GREEN + Style.BRIGHT + 'reached terminal velocity: {:>5.2f} == {:>5.2f}.'.format( _motor_velocity, _target_velocity)) # _motors.get_motor(Orientation.PORT).set_motor_power(_motor_power) time.sleep(_trial_time_sec) # decelerate... _target_velocity = 0.0 while _motor_velocity > _target_velocity: _motor_velocity = _limiter.slew(_motor_velocity, _target_velocity) _pid.set_velocity(_motor_velocity) _log.info(Fore.YELLOW + Style.NORMAL + 'velocity: {:>5.2f} -> {:>5.2f}.'.format( _motor_velocity, _target_velocity)) _rate.wait() _pid.disable() time.sleep(1.0) _motors.brake() # time.sleep(1.0) # _motors.get_motor(Orientation.STBD).accelerate_to_velocity(_motor_velocity, SlewRate.NORMAL, -1) # _stbd_pid.reset_steps() # _motors.get_motor(Orientation.STBD).set_motor_power(_motor_power) # _stbd_pid.enable() # time.sleep(_trial_time_sec) # _stbd_pid.disable() # _motors.brake() # output results of test: _log.info( Fore.MAGENTA + Style.BRIGHT + 'max diff steps: {:d} steps per loop @ motor velocity of {:5.2f}'. format(_pid.get_max_diff_steps(), _motor_velocity)) # _log.info(Fore.MAGENTA + Style.BRIGHT + 'STBD max diff steps: {:d} steps per loop @ motor velocity of {:5.2f}'.format(_stbd_pid.get_max_diff_steps(), _motor_velocity)) sys.exit(0) # _forward_steps = _fwd_steps_per_meter # _slew_rate = SlewRate.FAST # _direction = Direction.FORWARD # _velocity = Velocity.HALF # _log.info(Fore.RED + 'FORWARD... ---------------------------------- ') # go(_motors, _velocity, _slew_rate, Direction.FORWARD, _forward_steps) # _log.info(Fore.RED + 'BRAKING... ---------------------------------- ') # maintain velocity for 1 m # _distance_m = 2 # _port_step_limit = _port_pid.get_steps() + _fwd_steps_per_meter * _distance_m # _stbd_step_limit = _stbd_motor.get_steps() + _fwd_steps_per_meter * _distance_m # _port_pid.maintain_velocity(_velocity, _port_step_limit) # _stbd_motor.maintain_velocity(_velocity, _stbd_step_limit) # time.sleep(1.0) # _log.info(Fore.RED + 'REVERSE... ---------------------------------- ') # go(_motors, _velocity, _slew_rate, Direction.REVERSE, _forward_steps) # _port_pid.maintain_velocity(_velocity, _port_step_limit) # _stbd_motor.maintain_velocity(_velocity, _stbd_step_limit) # _motors.brake() _log.info(Fore.CYAN + Style.BRIGHT + 'completed thread.') # _log.info(Fore.CYAN + Style.BRIGHT + 'A. motor test complete; intended: {:d}; actual steps: port: {} ; stbd: {}.'.format(_forward_steps, _pid.get_steps(), _stbd_pid.get_steps())) _distance_cm = _pid.get_distance_cm_for_steps(_pid.get_steps()) _log.info(Fore.CYAN + Style.BRIGHT + 'distance traveled: {:>5.2f}.'.format(_distance_cm)) # _port_distance_cm = _port.get_distance_cm_for_steps(_pid.get_steps()) # _stbd_distance_cm = _stbd_pid.get_distance_cm_for_steps(_stbd_pid.get_steps()) # _log.info(Fore.CYAN + Style.BRIGHT + 'distance traveled: port: {:>5.2f}, stbd: {:>5.2f}.'.format(_port_distance_cm, _stbd_distance_cm)) except KeyboardInterrupt: _log.info(Fore.CYAN + Style.BRIGHT + 'B. motor test complete; intended: {:d}; actual steps: port: {} ; stbd: {}.'.format(\ _forward_steps, _port_pid.get_steps(), _stbd_pid.get_steps())) _stbd_motor.halt() except Exception as e: _log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}'.format(e)) traceback.print_exc(file=sys.stdout) finally: _log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.') # _stbd_motor.halt() quit()