utils.spark_max_callbacks

  1# Third-party imports
  2import rev
  3
  4
  5class SparkMaxCallbacks:
  6    """
  7    Wraps a REV SparkMax motor and encoder into a dict of callbacks
  8    for PositionCalibration.
  9
 10    You usually don't need to use this class directly. Pass
 11    ``motor=`` to PositionCalibration and it creates these
 12    callbacks for you automatically.
 13
 14    Direct usage (if you want to customize one callback)::
 15
 16        # Start from SparkMax defaults, then override one
 17        cbs = SparkMaxCallbacks(motor, encoder).as_dict()
 18        cbs['on_limit_detected'] = my_custom_handler
 19        cal = PositionCalibration(name="Turret", ..., **cbs)
 20
 21    Returned callbacks and what they wrap:
 22
 23    ============================  ======================================
 24    Callback name                 SparkMax call
 25    ============================  ======================================
 26    ``set_motor_output``          ``motor.set(pct)``
 27    ``stop_motor``                ``motor.stopMotor()``
 28    ``set_position``              ``encoder.setPosition(value)``
 29    ``get_position``              ``encoder.getPosition()``
 30    ``get_velocity``              ``encoder.getVelocity()``
 31    ``get_forward_limit_switch``  ``motor.getForwardLimitSwitch().get()``
 32    ``get_reverse_limit_switch``  ``motor.getReverseLimitSwitch().get()``
 33    ``set_current_limit``         ``motor.configure(smartCurrentLimit)``
 34    ``set_soft_limits``           ``motor.configure(softLimit ...)``
 35    ``disable_soft_limits``       ``motor.configure(softLimit ...)``
 36    ``save_config``               reads ``motor.configAccessor``
 37    ``restore_config``            ``motor.configure(...)``
 38    ``set_conversion_factor``     ``motor.configure(encoder ...)``
 39    ============================  ======================================
 40    """
 41
 42    def __init__(self, motor: rev.SparkMax, encoder=None):
 43        self._motor = motor
 44        self._encoder = encoder or motor.getEncoder()
 45
 46    def as_dict(self) -> dict:
 47        """Return all callbacks as a dict for PositionCalibration(**kwargs)."""
 48        return {
 49            'get_position': lambda: self._encoder.getPosition(),
 50            'get_velocity': lambda: self._encoder.getVelocity(),
 51            'set_position': lambda v: self._encoder.setPosition(v),
 52            'set_motor_output': lambda p: self._motor.set(p),
 53            'stop_motor': lambda: self._motor.stopMotor(),
 54            'set_current_limit': self._make_set_current_limit(),
 55            'set_soft_limits': self._make_set_soft_limits(),
 56            'disable_soft_limits': self._make_disable_soft_limits(),
 57            'save_config': self._make_save_config(),
 58            'restore_config': self._make_restore_config(),
 59            'get_forward_limit_switch': (
 60                lambda: self._motor.getForwardLimitSwitch().get()
 61            ),
 62            'get_reverse_limit_switch': (
 63                lambda: self._motor.getReverseLimitSwitch().get()
 64            ),
 65            'set_conversion_factor': (
 66                self._make_set_conversion_factor()
 67            ),
 68        }
 69
 70    def _make_set_current_limit(self):
 71        motor = self._motor
 72
 73        def set_current_limit(amps):
 74            cfg = rev.SparkMaxConfig()
 75            cfg.smartCurrentLimit(int(amps))
 76            motor.configure(
 77                cfg,
 78                rev.ResetMode.kNoResetSafeParameters,
 79                rev.PersistMode.kNoPersistParameters
 80            )
 81        return set_current_limit
 82
 83    def _make_set_soft_limits(self):
 84        motor = self._motor
 85
 86        def set_soft_limits(min_limit, max_limit):
 87            cfg = rev.SparkMaxConfig()
 88            (
 89                cfg.softLimit
 90                .forwardSoftLimit(max_limit)
 91                .forwardSoftLimitEnabled(True)
 92                .reverseSoftLimit(min_limit)
 93                .reverseSoftLimitEnabled(True)
 94            )
 95            motor.configure(
 96                cfg,
 97                rev.ResetMode.kNoResetSafeParameters,
 98                rev.PersistMode.kNoPersistParameters
 99            )
100        return set_soft_limits
101
102    def _make_disable_soft_limits(self):
103        motor = self._motor
104
105        def disable_soft_limits(forward, reverse):
106            cfg = rev.SparkMaxConfig()
107            if forward:
108                cfg.softLimit.forwardSoftLimitEnabled(False)
109            if reverse:
110                cfg.softLimit.reverseSoftLimitEnabled(False)
111            motor.configure(
112                cfg,
113                rev.ResetMode.kNoResetSafeParameters,
114                rev.PersistMode.kNoPersistParameters
115            )
116        return disable_soft_limits
117
118    def _make_save_config(self):
119        motor = self._motor
120
121        def save_config():
122            ca = motor.configAccessor
123            return {
124                'current_limit': ca.getSmartCurrentLimit(),
125                'current_free_limit': ca.getSmartCurrentFreeLimit(),
126                'current_rpm_limit': ca.getSmartCurrentRPMLimit(),
127                'fwd_soft_limit_enabled': (
128                    ca.softLimit.getForwardSoftLimitEnabled()
129                ),
130                'rev_soft_limit_enabled': (
131                    ca.softLimit.getReverseSoftLimitEnabled()
132                ),
133                'fwd_soft_limit': ca.softLimit.getForwardSoftLimit(),
134                'rev_soft_limit': ca.softLimit.getReverseSoftLimit(),
135            }
136        return save_config
137
138    def _make_restore_config(self):
139        motor = self._motor
140
141        def restore_config(saved):
142            cfg = rev.SparkMaxConfig()
143            cfg.smartCurrentLimit(
144                int(saved['current_limit']),
145                int(saved['current_free_limit']),
146                int(saved['current_rpm_limit'])
147            )
148            (
149                cfg.softLimit
150                .forwardSoftLimitEnabled(saved['fwd_soft_limit_enabled'])
151                .reverseSoftLimitEnabled(saved['rev_soft_limit_enabled'])
152            )
153            motor.configure(
154                cfg,
155                rev.ResetMode.kNoResetSafeParameters,
156                rev.PersistMode.kNoPersistParameters
157            )
158        return restore_config
159
160    def _make_set_conversion_factor(self):
161        motor = self._motor
162
163        def set_conversion_factor(factor):
164            # Velocity factor mirrors the position factor scaled by 1/60
165            # so that getVelocity() reads user-units per second.
166            velocity_factor = factor / 60.0
167            cfg = rev.SparkMaxConfig()
168            (
169                cfg.encoder
170                .positionConversionFactor(factor)
171                .velocityConversionFactor(velocity_factor)
172            )
173            motor.configure(
174                cfg,
175                rev.ResetMode.kNoResetSafeParameters,
176                rev.PersistMode.kNoPersistParameters
177            )
178        return set_conversion_factor