diff --git a/XRPLib/board.py b/XRPLib/board.py index 1a17ead..69a5be8 100644 --- a/XRPLib/board.py +++ b/XRPLib/board.py @@ -7,6 +7,10 @@ class Board: _DEFAULT_BOARD_INSTANCE = None + # Nominal pack voltage the motor gains are tuned against; voltage_scale corrects for the + # pack actually installed (effort is raw PWM duty, so torque scales with voltage). + _nominal_voltage = 4.2 if "NanoXRP" in sys.implementation._machine else 6.0 + @classmethod def get_default_board(cls): """ @@ -28,7 +32,11 @@ def __init__(self, vin_pin="BOARD_VIN_MEASURE", button_pin="BOARD_USER_BUTTON", """ self.on_switch = ADC(Pin(vin_pin)) - + + # Measure the pack once so motor controllers can compensate effort for battery droop. + self.voltage_scale = 1.0 + self.update_voltage_compensation() + self.button = Pin(button_pin, Pin.IN, Pin.PULL_UP) time.sleep(.01) # give some time for the pull up to get to the proper voltage, otherwise the button could read as pressed @@ -131,6 +139,18 @@ def set_rgb_led(self, r:int, g:int, b:int): else: raise NotImplementedError("Board.set_rgb_led not implemented for the XRP Beta") + def update_voltage_compensation(self) -> float: + """ + Re-measure the battery and update voltage_scale, the factor motor controllers apply to + effort so torque stays consistent as the pack drains. Call again after a battery swap. + + :return: The effort scale now in use + :rtype: float + """ + voltage = sum(self.get_battery_voltage() for _ in range(8)) / 8 + self.voltage_scale = min(max(self._nominal_voltage / max(voltage, 3.5), 0.7), 1.6) + return self.voltage_scale + def get_battery_voltage(self, vin_pin="BOARD_VIN_MEASURE") -> float: """ Returns the current battery voltage in volts. diff --git a/XRPLib/differential_drive.py b/XRPLib/differential_drive.py index cd803e1..1b39934 100644 --- a/XRPLib/differential_drive.py +++ b/XRPLib/differential_drive.py @@ -68,15 +68,9 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU else: self.wheel_track = wheel_track - # Effort is raw PWM duty, so torque scales with pack voltage. Gains are tuned against - # nominal_voltage and voltage_scale corrects the duty for the pack actually installed. - if "NanoXRP" in implementation._machine: - self.nominal_voltage = 4.2 - else: - self.nominal_voltage = 6.0 - - self.voltage_scale = 1.0 - self.update_voltage_compensation() + # Battery compensation lives on Board (measured once, shared). straight()/turn() read + # board.voltage_scale so effort tracks the pack as it drains. + self._board = Board.get_default_board() self.heading_pid = None self.current_heading = None @@ -86,23 +80,6 @@ def __init__(self, left_motor: EncodedMotor, right_motor: EncodedMotor, imu: IMU if self.imu: self.heading_pid = PID( kp = 0.075, kd=0.001, ) - def update_voltage_compensation(self) -> float: - """ - Re-measures the battery and updates the effort scale applied to straight() and turn(). - Called at construction; call it again after a battery swap or a long run. - - :return: The effort scale now in use - :rtype: float - """ - if self.nominal_voltage is None: - return self.voltage_scale - - board = Board.get_default_board() - voltage = sum(board.get_battery_voltage() for _ in range(8)) / 8 - self.voltage_scale = min(max(self.nominal_voltage / max(voltage, 3.5), 0.7), 1.6) - - return self.voltage_scale - def set_effort(self, left_effort: float, right_effort: float) -> None: """ Set the raw effort of both motors individually @@ -126,9 +103,9 @@ def set_speed(self, left_speed: float, right_speed: float) -> None: :type rightSpeed: float """ # Convert from cm/s to RPM - cmpsToRPM = 60 / (math.pi * self.wheel_diam) - self.left_motor.set_speed(left_speed*cmpsToRPM) - self.right_motor.set_speed(right_speed*cmpsToRPM) + cmps_to_rpm = 60 / (math.pi * self.wheel_diam) + self.left_motor.set_speed(left_speed*cmps_to_rpm) + self.right_motor.set_speed(right_speed*cmps_to_rpm) def set_zero_effort_behavior(self, brake_at_zero_effort): @@ -308,7 +285,7 @@ def _move(self, distance_target: float, heading_target: float, max_effort: float elif correcting and 0 < effort < min_effort: left, right = left * min_effort / effort, right * min_effort / effort - self.set_effort(left * self.voltage_scale, right * self.voltage_scale) + self.set_effort(left * self._board.voltage_scale, right * self._board.voltage_scale) time.sleep(0.01) diff --git a/XRPLib/encoded_motor.py b/XRPLib/encoded_motor.py index da3ece7..b5fb511 100644 --- a/XRPLib/encoded_motor.py +++ b/XRPLib/encoded_motor.py @@ -3,6 +3,7 @@ from machine import Timer, Pin from .controller import Controller from .pid import PID +from .board import Board from sys import implementation class EncodedMotor: @@ -10,6 +11,10 @@ class EncodedMotor: ZERO_EFFORT_BREAK = True ZERO_EFFORT_COAST = False + # Speed control runs on a fixed-period timer, and the rpm <-> counts conversions depend on that period. + _UPDATE_PERIOD_MS = 20 + _UPDATE_HZ = 1000 // _UPDATE_PERIOD_MS + _DEFAULT_LEFT_MOTOR_INSTANCE = None _DEFAULT_RIGHT_MOTOR_INSTANCE = None _DEFAULT_MOTOR_THREE_INSTANCE = None @@ -70,29 +75,29 @@ def __init__(self, motor, encoder: Encoder): self.brake_at_zero = False self.target_speed = None + + # Velocity control = feedforward (kS breaks stiction, kV per unit speed) plus a proportional trim. if "NanoXRP" in implementation._machine: - self.DEFAULT_SPEED_CONTROLLER = PID( - kp=0.015, - ki=0.06, - kd=0, - max_integral=1/0.06 - ) + self.kS = 0.1 + self.kV = 0.00122 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.005) else: - self.DEFAULT_SPEED_CONTROLLER = PID( - kp=0.035, - ki=0.03, - kd=0, - max_integral=50 - ) + self.kS = 0.12 + self.kV = 0.02 + self.DEFAULT_SPEED_CONTROLLER = PID(kp=0.1) self.speedController = self.DEFAULT_SPEED_CONTROLLER + + # voltage_scale is measured once when Board is constructed; just hold a reference. + self._board = Board.get_default_board() + self.prev_position = 0 - self.speed = 0 + self._counts_per_update = 0 # encoder counts moved in the last update period self.prev_speed = 0 # Use a virtual timer so we can leave the hardware timers up for the user self.updateTimer = Timer(-1) - # If the update timer is not running, start it at 50 Hz (20ms updates) - self.updateTimer.init(period=20, callback=lambda t:self._update()) + # If the update timer is not running, start it at the update rate + self.updateTimer.init(period=self._UPDATE_PERIOD_MS, callback=lambda t:self._update()) def set_effort(self, effort: float): @@ -155,13 +160,20 @@ def reset_encoder_position(self): """ self._encoder.reset_encoder_position() + def _counts_per_update_to_rpm(self, counts: float) -> float: + # counts moved in one update period -> revolutions per minute + return counts * 60 * self._UPDATE_HZ / self._encoder.resolution + + def _rpm_to_counts_per_update(self, rpm: float) -> float: + # revolutions per minute -> counts moved in one update period + return rpm * self._encoder.resolution / (60 * self._UPDATE_HZ) + def get_speed(self) -> float: """ :return: The speed of the motor, in rpm :rtype: float """ - # Convert from counts per 20ms to rpm (60 sec/min, 50 Hz) - return self.speed*(60*50)/self._encoder.resolution + return self._counts_per_update_to_rpm(self._counts_per_update) def set_speed(self, speed_rpm: float = None): """ @@ -173,6 +185,7 @@ def set_speed(self, speed_rpm: float = None): """ if speed_rpm is None or speed_rpm == 0: self.target_speed = None + self.prev_speed = 0 # forget direction; the controller is cleared right below self.set_effort(0) self.speedController.clear_history() return @@ -182,8 +195,7 @@ def set_speed(self, speed_rpm: float = None): self.prev_speed = speed_rpm - # Convert from rev per min to counts per 20ms (60 sec/min, 50 Hz) - self.target_speed = speed_rpm*self._encoder.resolution/(60*50) + self.target_speed = self._rpm_to_counts_per_update(speed_rpm) def set_speed_controller(self, new_controller: Controller): """ @@ -200,9 +212,10 @@ def _update(self): Non-api method; used for updating motor efforts for speed control """ current_position = self.get_position_counts() - self.speed = current_position - self.prev_position + self._counts_per_update = current_position - self.prev_position if self.target_speed is not None: - error = self.target_speed - self.speed - effort = self.speedController.update(error) - self._motor.set_effort(effort) + error = self.target_speed - self._counts_per_update + feedforward = (self.kS if self.target_speed > 0 else -self.kS) + self.kV * self.target_speed + effort = (feedforward + self.speedController.update(error)) * self._board.voltage_scale + self._motor.set_effort(max(-1.0, min(1.0, effort))) self.prev_position = current_position