subsystem

 8class Hopper(SimpleMotorSubsystem):
 9    def __init__(self):
10        super().__init__(HopperConstants)

 31class IntakePosition(Subsystem):
 32    """Servo-type intake arm with position control and velocity limits."""
 33
 34    # -- Telemetry --
 35    nt_position = ntproperty(f"{_NT}/position", 0.0)
 36    nt_velocity = ntproperty(f"{_NT}/velocity", 0.0)
 37    nt_applied_output = ntproperty(f"{_NT}/appliedOutput", 0.0)
 38    nt_current = ntproperty(f"{_NT}/current", 0.0)
 39    nt_bus_voltage = ntproperty(f"{_NT}/busVoltage", 0.0)
 40    nt_temperature = ntproperty(f"{_NT}/temperature", 0.0)
 41    nt_target_position = ntproperty(f"{_NT}/targetPosition", 0.0)
 42    nt_at_target = ntproperty(f"{_NT}/atTargetPosition", False)
 43    nt_min_soft_limit = ntproperty(f"{_NT}/minSoftLimit", 0.0)
 44    nt_max_soft_limit = ntproperty(f"{_NT}/maxSoftLimit", 0.0)
 45    nt_forward_limit_hit = ntproperty(f"{_NT}/forwardLimitHit", False)
 46    nt_reverse_limit_hit = ntproperty(f"{_NT}/reverseLimitHit", False)
 47    nt_is_calibrated = ntproperty(f"{_NT}/isCalibrated", False)
 48
 49    # -- Tunable motion profile parameters --
 50    nt_max_velocity = ntproperty(f"{_NT}/pid/maxVelocityDegS", 90.0)
 51    nt_max_acceleration = ntproperty(f"{_NT}/pid/maxAccelDegS2", 180.0)
 52    nt_min_output_voltage = ntproperty(f"{_NT}/pid/minOutputVoltage", -4.0)
 53    nt_max_output_voltage = ntproperty(f"{_NT}/pid/maxOutputVoltage", 4.0)
 54    nt_kV = ntproperty(f"{_NT}/pid/kV", 4.0 / 90.0)
 55    nt_kS = ntproperty(f"{_NT}/pid/kS", 0.0)
 56    nt_kG = ntproperty(f"{_NT}/pid/kG", 0.0)
 57    nt_arm_zero_angle_deg = ntproperty(f"{_NT}/pid/armZeroAngleDeg", -15.0)
 58
 59    def __init__(self) -> None:
 60        super().__init__()
 61
 62        # Motor setup
 63        motor_class = getattr(consts, 'motorClass', rev.SparkMax)
 64        self.motor = motor_class(
 65            consts.canId,
 66            rev.SparkLowLevel.MotorType.kBrushless,
 67        )
 68        self.encoder = self.motor.getEncoder()
 69        self.position_conversion_factor = consts.positionConversionFactor
 70
 71        # Internal state
 72        self._kS = 0.0
 73        self._kG = 0.0
 74        self._kV = 4.0 / 90.0
 75        self._arm_zero_angle_deg = -15.0
 76        self._max_velocity = 90.0
 77        self._max_acceleration = 180.0
 78        self._min_output_voltage = -4.0
 79        self._max_output_voltage = 4.0
 80        self._target_position = None
 81        self._is_calibrated = False
 82
 83        # Profiled PID with trapezoidal motion constraints
 84        self.controller = ProfiledPIDController(
 85            0.05, 0, 0,
 86            TrapezoidProfile.Constraints(
 87                self._max_velocity, self._max_acceleration
 88            )
 89        )
 90        self.controller.setTolerance(1.0, 5.0)
 91        wpilib.SmartDashboard.putData(
 92            self.getName() + "/pid", self.controller)
 93
 94        # Calibration
 95        self.calibration = self._setup_calibration(
 96            consts.minSoftLimit, consts.maxSoftLimit)
 97
 98        # Mechanism2d visualization
 99        self._configure_mechanism2d()
100
101        # Motor configuration
102        cfg = rev.SparkBaseConfig()
103        vel_conv = consts.positionConversionFactor / 60.0
104        (
105            cfg
106            .inverted(consts.inverted)
107            .setIdleMode(rev.SparkBaseConfig.IdleMode.kBrake)
108            .voltageCompensation(12.0)
109            .smartCurrentLimit(consts.currentLimitAmps)
110        )
111        (
112            cfg.encoder
113            .positionConversionFactor(consts.positionConversionFactor)
114            .velocityConversionFactor(vel_conv)
115        )
116        # Soft limits start disabled so calibration can drive freely
117        (
118            cfg.softLimit
119            .forwardSoftLimit(consts.maxSoftLimit)
120            .forwardSoftLimitEnabled(False)
121            .reverseSoftLimit(consts.minSoftLimit)
122            .reverseSoftLimitEnabled(False)
123        )
124        GetSparkSignalsPositionControlConfig(cfg.signals, 20)
125        self.motor.configure(
126            cfg,
127            rev.ResetMode.kResetSafeParameters,
128            rev.PersistMode.kNoPersistParameters,
129        )
130
131        self.setName(consts.name)
132
133    def _setup_calibration(
134        self, min_soft_limit: float, max_soft_limit: float,
135    ) -> PositionCalibration:
136        cal = PositionCalibration(
137            name=self.getName(),
138            fallback_min=min_soft_limit,
139            fallback_max=max_soft_limit,
140        )
141        spark_cbs = SparkMaxCallbacks(self.motor, self.encoder).as_dict()
142        cal.set_callbacks(
143            set_motor_output=spark_cbs['set_motor_output'],
144            stop_motor=spark_cbs['stop_motor'],
145            set_position=spark_cbs['set_position'],
146            get_velocity=spark_cbs['get_velocity'],
147            get_forward_limit_switch=spark_cbs['get_forward_limit_switch'],
148            get_reverse_limit_switch=spark_cbs['get_reverse_limit_switch'],
149            get_position=spark_cbs['get_position'],
150            set_current_limit=spark_cbs['set_current_limit'],
151            set_soft_limits=spark_cbs['set_soft_limits'],
152            disable_soft_limits=spark_cbs['disable_soft_limits'],
153            save_config=spark_cbs['save_config'],
154            restore_config=spark_cbs['restore_config'],
155        )
156        return cal
157
158    def _configure_mechanism2d(self) -> None:
159        """Mechanism2d arm visualization.
160
161        display_angle = 90 + encoder_position (CCW sweep as arm deploys).
162        """
163        self.mech2d = wpilib.Mechanism2d(300, 250)
164        pivot = self.mech2d.getRoot("intake_pivot", 200, 80)
165        pivot.appendLigament(
166            "ref_vertical", 80, 90, 1,
167            wpilib.Color8Bit(wpilib.Color.kGray))
168        pivot.appendLigament(
169            "ref_horizontal", 80, 180, 1,
170            wpilib.Color8Bit(wpilib.Color.kGray))
171        self.mech_min_limit_arm = pivot.appendLigament(
172            "stowed_limit", 80, 90 + consts.minSoftLimit, 4,
173            wpilib.Color8Bit(0, 0, 255))
174        self.mech_max_limit_arm = pivot.appendLigament(
175            "deployed_limit", 80, 90 + consts.maxSoftLimit, 4,
176            wpilib.Color8Bit(255, 140, 0))
177        self.mech_current_arm = pivot.appendLigament(
178            "current", 80, 90 + consts.minSoftLimit, 3,
179            wpilib.Color8Bit(wpilib.Color.kRed))
180        self.mech_target_arm = pivot.appendLigament(
181            "target", 0, 90, 5,
182            wpilib.Color8Bit(wpilib.Color.kYellow))
183        wpilib.SmartDashboard.putData(
184            self.getName() + "/mechanism", self.mech2d)
185
186    # -- Convenience API (preserves old IntakeSubsystem interface) --
187
188    def deployIntake(self) -> None:
189        """Move arm to deployed position."""
190        self.setPosition(consts.deployedPosition)
191
192    def stowIntake(self) -> None:
193        """Move arm to stowed position."""
194        self.setPosition(consts.stowedPosition)
195
196    def isIntakeDeployed(self) -> bool:
197        """True when arm is at or past the deployed position."""
198        return self.encoder.getPosition() >= consts.deployedPosition - 1.0
199
200    def isIntakeStowed(self) -> bool:
201        """True when arm is at or before the stowed position."""
202        return self.encoder.getPosition() <= consts.stowedPosition + 1.0
203
204    # -- Core API --
205
206    def calibrationInit(
207        self,
208        max_current: float = 10.0,
209        max_power_pct: float = 0.05,
210        max_homing_time: float = 10.0,
211    ) -> None:
212        """Start two-phase calibration at low power."""
213        self._is_calibrated = False
214        self.calibration.calibration_init(
215            max_current=max_current,
216            max_power_pct=max_power_pct,
217            max_homing_time=max_homing_time,
218        )
219
220    def setPosition(self, position_degrees: float) -> None:
221        """Set target arm position. Blocked during calibration."""
222        if self.calibration.is_busy:
223            return
224        self._target_position = max(
225            self.calibration.min_soft_limit,
226            min(self.calibration.max_soft_limit, position_degrees)
227        )
228
229    def getPosition(self) -> float:
230        return self.encoder.getPosition()
231
232    def getVelocity(self) -> float:
233        return self.encoder.getVelocity()
234
235    def disable(self) -> None:
236        """Stop arm and clear target."""
237        self._target_position = None
238        self.motor.stopMotor()
239
240    def setMotorVoltage(self, output: float) -> None:
241        """Direct voltage for SysId. Blocked during calibration."""
242        if self.calibration.is_busy:
243            return
244        self.motor.setVoltage(output)
245
246    # -- Periodic --
247
248    def periodic(self) -> None:
249        if self.calibration.is_busy:
250            self.calibration.periodic()
251            if not self.calibration.is_busy:
252                self._is_calibrated = self.calibration.is_calibrated
253        elif self._target_position is not None:
254            position = self.encoder.getPosition()
255            pid_output = self.controller.calculate(
256                position, self._target_position)
257
258            profile_velocity = self.controller.getSetpoint().velocity
259
260            # Gravity feedforward
261            arm_angle_rad = math.radians(
262                position + self._arm_zero_angle_deg)
263            kG_component = self._kG * math.sin(arm_angle_rad)
264
265            kS_component = (
266                self._kS * math.copysign(1.0, profile_velocity)
267                if abs(profile_velocity) > 1e-6 else 0.0
268            )
269            feedforward = (
270                kS_component + kG_component
271                + self._kV * profile_velocity
272            )
273
274            if self.controller.atGoal():
275                self.motor.setVoltage(
276                    max(self._min_output_voltage,
277                        min(self._max_output_voltage, kG_component)))
278            else:
279                output = max(
280                    self._min_output_voltage,
281                    min(self._max_output_voltage, pid_output + feedforward))
282                self.motor.setVoltage(output)
283        else:
284            self.motor.stopMotor()
285
286        self._updateTelemetry()
287
288    def _updateTelemetry(self) -> None:
289        self.nt_position = self.encoder.getPosition()
290        self.nt_velocity = self.encoder.getVelocity()
291        self.nt_applied_output = self.motor.getAppliedOutput()
292        self.nt_current = self.motor.getOutputCurrent()
293        self.nt_bus_voltage = self.motor.getBusVoltage()
294        self.nt_temperature = self.motor.getMotorTemperature()
295        self.nt_target_position = (
296            self._target_position if self._target_position is not None
297            else 0.0)
298        self.nt_at_target = (
299            self._target_position is not None
300            and self.controller.atGoal())
301        self.nt_is_calibrated = self._is_calibrated
302
303        sl = self.motor.configAccessor.softLimit
304        self.nt_min_soft_limit = sl.getReverseSoftLimit()
305        self.nt_max_soft_limit = sl.getForwardSoftLimit()
306        self.nt_forward_limit_hit = self.motor.getForwardLimitSwitch().get()
307        self.nt_reverse_limit_hit = self.motor.getReverseLimitSwitch().get()
308
309        # Read back live-tunable parameters
310        self._min_output_voltage = self.nt_min_output_voltage
311        self._max_output_voltage = self.nt_max_output_voltage
312        self._kS = self.nt_kS
313        self._kG = self.nt_kG
314        self._kV = self.nt_kV
315        self._arm_zero_angle_deg = self.nt_arm_zero_angle_deg
316
317        new_vel = self.nt_max_velocity
318        new_accel = self.nt_max_acceleration
319        if new_vel != self._max_velocity or new_accel != self._max_acceleration:
320            self._max_velocity = new_vel
321            self._max_acceleration = new_accel
322            self.controller.setConstraints(
323                TrapezoidProfile.Constraints(new_vel, new_accel))
324
325        self.calibration.update_telemetry(self.getName() + "/")
326
327        # Mechanism2d
328        self.mech_current_arm.setAngle(90 + self.encoder.getPosition())
329        self.mech_min_limit_arm.setAngle(90 + consts.minSoftLimit)
330        self.mech_max_limit_arm.setAngle(90 + consts.maxSoftLimit)
331        if self._target_position is not None:
332            self.mech_target_arm.setLength(80)
333            self.mech_target_arm.setAngle(90 + self._target_position)
334        else:
335            self.mech_target_arm.setLength(0)
336
337    # -- SysId --
338
339    def sysIdLog(self, sys_id_routine: SysIdRoutineLog) -> None:
340        motor_log = sys_id_routine.motor("intake_pos")
341        angular_position = math.radians(self.encoder.getPosition())
342        angular_velocity = math.radians(self.encoder.getVelocity())
343        battery_voltage = self.motor.getBusVoltage()
344        applied_voltage = self.motor.getAppliedOutput() * battery_voltage
345        motor_log.angularPosition(angular_position)
346        motor_log.angularVelocity(angular_velocity)
347        motor_log.current(self.motor.getOutputCurrent())
348        motor_log.voltage(applied_voltage)
349        motor_log.value("temperature", self.motor.getMotorTemperature(), "C")
350        motor_log.value("busVoltage", battery_voltage, "V")
351
352    def sysIdQuasistaticCommand(
353        self, direction: SysIdRoutine.Direction,
354        sysIdRoutine: SysIdRoutine,
355    ) -> Command:
356        return sysIdRoutine.quasistatic(direction)
357
358    def sysIdDynamicCommand(
359        self, direction: SysIdRoutine.Direction,
360        sysIdRoutine: SysIdRoutine,
361    ) -> Command:
362        return sysIdRoutine.dynamic(direction)

 8class IntakeRoller(SimpleMotorSubsystem):
 9    def __init__(self):
10        super().__init__(IntakeRollerConstants)