Feedforward speed control with shared battery compensation

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.

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)
 

XRPLib/encoded_motor.py

@@ -3,13 +3,18 @@
 from machine import Timer, Pin
 from .controller import Controller
 from .pid import PID
+from .board import Board
 from sys import implementation
 
 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