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    ============================  ======================================
 39    """
 40
 41    def __init__(self, motor: rev.SparkMax, encoder=None):
 42        self._motor = motor
 43        self._encoder = encoder or motor.getEncoder()
 44
 45    def as_dict(self) -> dict:
 46        """Return all callbacks as a dict for PositionCalibration(**kwargs)."""
 47        return {
 48            'get_position': lambda: self._encoder.getPosition(),
 49            'get_velocity': lambda: self._encoder.getVelocity(),
 50            'set_position': lambda v: self._encoder.setPosition(v),
 51            'set_motor_output': lambda p: self._motor.set(p),
 52            'stop_motor': lambda: self._motor.stopMotor(),
 53            'set_current_limit': self._make_set_current_limit(),
 54            'set_soft_limits': self._make_set_soft_limits(),
 55            'disable_soft_limits': self._make_disable_soft_limits(),
 56            'save_config': self._make_save_config(),
 57            'restore_config': self._make_restore_config(),
 58            'get_forward_limit_switch': (
 59                lambda: self._motor.getForwardLimitSwitch().get()
 60            ),
 61            'get_reverse_limit_switch': (
 62                lambda: self._motor.getReverseLimitSwitch().get()
 63            ),
 64        }
 65
 66    def _make_set_current_limit(self):
 67        motor = self._motor
 68
 69        def set_current_limit(amps):
 70            cfg = rev.SparkMaxConfig()
 71            cfg.smartCurrentLimit(int(amps))
 72            motor.configure(
 73                cfg,
 74                rev.ResetMode.kNoResetSafeParameters,
 75                rev.PersistMode.kNoPersistParameters
 76            )
 77        return set_current_limit
 78
 79    def _make_set_soft_limits(self):
 80        motor = self._motor
 81
 82        def set_soft_limits(min_limit, max_limit):
 83            cfg = rev.SparkMaxConfig()
 84            (
 85                cfg.softLimit
 86                .forwardSoftLimit(max_limit)
 87                .forwardSoftLimitEnabled(True)
 88                .reverseSoftLimit(min_limit)
 89                .reverseSoftLimitEnabled(True)
 90            )
 91            motor.configure(
 92                cfg,
 93                rev.ResetMode.kNoResetSafeParameters,
 94                rev.PersistMode.kNoPersistParameters
 95            )
 96        return set_soft_limits
 97
 98    def _make_disable_soft_limits(self):
 99        motor = self._motor
100
101        def disable_soft_limits(forward, reverse):
102            cfg = rev.SparkMaxConfig()
103            if forward:
104                cfg.softLimit.forwardSoftLimitEnabled(False)
105            if reverse:
106                cfg.softLimit.reverseSoftLimitEnabled(False)
107            motor.configure(
108                cfg,
109                rev.ResetMode.kNoResetSafeParameters,
110                rev.PersistMode.kNoPersistParameters
111            )
112        return disable_soft_limits
113
114    def _make_save_config(self):
115        motor = self._motor
116
117        def save_config():
118            ca = motor.configAccessor
119            return {
120                'current_limit': ca.getSmartCurrentLimit(),
121                'current_free_limit': ca.getSmartCurrentFreeLimit(),
122                'current_rpm_limit': ca.getSmartCurrentRPMLimit(),
123                'fwd_soft_limit_enabled': (
124                    ca.softLimit.getForwardSoftLimitEnabled()
125                ),
126                'rev_soft_limit_enabled': (
127                    ca.softLimit.getReverseSoftLimitEnabled()
128                ),
129                'fwd_soft_limit': ca.softLimit.getForwardSoftLimit(),
130                'rev_soft_limit': ca.softLimit.getReverseSoftLimit(),
131            }
132        return save_config
133
134    def _make_restore_config(self):
135        motor = self._motor
136
137        def restore_config(saved):
138            cfg = rev.SparkMaxConfig()
139            cfg.smartCurrentLimit(
140                int(saved['current_limit']),
141                int(saved['current_free_limit']),
142                int(saved['current_rpm_limit'])
143            )
144            (
145                cfg.softLimit
146                .forwardSoftLimitEnabled(saved['fwd_soft_limit_enabled'])
147                .reverseSoftLimitEnabled(saved['rev_soft_limit_enabled'])
148            )
149            motor.configure(
150                cfg,
151                rev.ResetMode.kNoResetSafeParameters,
152                rev.PersistMode.kNoPersistParameters
153            )
154        return restore_config