83 lines
3.7 KiB
Python
83 lines
3.7 KiB
Python
"""
|
||
components/hopper.py – HopperSubsystem ported to MagicBot component.
|
||
|
||
Hardware:
|
||
- TalonFX spindexer motor
|
||
- TalonFX kicker motor
|
||
"""
|
||
|
||
import magicbot
|
||
from phoenix6.configs import MotorOutputConfigs, Slot0Configs, TalonFXConfiguration
|
||
from phoenix6.hardware import TalonFX
|
||
|
||
import constants
|
||
|
||
|
||
class HopperComponent:
|
||
"""MagicBot component that controls the hopper (spindexer + kicker)."""
|
||
|
||
def setup(self) -> None:
|
||
"""
|
||
MagicBot lifecycle hook — instantiate and configure all hopper hardware.
|
||
|
||
Called once by MagicBot after all component objects are created but
|
||
before the robot enters any enabled mode.
|
||
"""
|
||
# ── Spindexer ──────────────────────────────────────────────────────
|
||
self._spindexer_motor = TalonFX(constants.HopperConstants.SPINDEXER_MOTOR_ID)
|
||
spindexer_cfg = TalonFXConfiguration()
|
||
spindexer_cfg.motor_output = MotorOutputConfigs().with_inverted(
|
||
constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION
|
||
)
|
||
spindexer_cfg.slot0 = Slot0Configs().with_k_p(0).with_k_i(0).with_k_d(0).with_k_s(0)
|
||
self._spindexer_motor.configurator.apply(spindexer_cfg)
|
||
|
||
# ── Kicker ─────────────────────────────────────────────────────────
|
||
self._kicker_motor = TalonFX(constants.HopperConstants.KICKER_MOTOR_ID)
|
||
kicker_cfg = TalonFXConfiguration()
|
||
kicker_cfg.motor_output = MotorOutputConfigs().with_inverted(
|
||
constants.HopperConstants.KICKER_MOTOR_DIRECTION
|
||
)
|
||
kicker_cfg.slot0 = Slot0Configs().with_k_p(0).with_k_i(0).with_k_d(0).with_k_s(0)
|
||
self._kicker_motor.configurator.apply(kicker_cfg)
|
||
|
||
self._kicker_velocity = self._kicker_motor.get_velocity()
|
||
|
||
# ── Public API ─────────────────────────────────────────────────────────────
|
||
|
||
def feed_kicker(self) -> None:
|
||
"""Drive the kicker motor forward at 75 % output to push a note into the launcher."""
|
||
self._kicker_motor.set(0.75)
|
||
|
||
def reverse_kicker(self) -> None:
|
||
"""Drive the kicker motor in reverse at 50 % output to un-jam a stuck note."""
|
||
self._kicker_motor.set(-0.5)
|
||
|
||
def stop_kicker(self) -> None:
|
||
"""Stop the kicker motor."""
|
||
self._kicker_motor.stop_motor()
|
||
|
||
def spindexer_on(self) -> None:
|
||
"""Drive the spindexer forward at 75 % output to advance notes toward the kicker."""
|
||
self._spindexer_motor.set(0.75)
|
||
|
||
def spindexer_reverse(self) -> None:
|
||
"""Drive the spindexer in reverse at 75 % output to clear a jam."""
|
||
self._spindexer_motor.set(-0.75)
|
||
|
||
def spindexer_off(self) -> None:
|
||
"""Stop the spindexer motor."""
|
||
self._spindexer_motor.stop_motor()
|
||
|
||
# ── Telemetry ──────────────────────────────────────────────────────────────
|
||
|
||
@magicbot.feedback
|
||
def kicker_speed_rps(self) -> float:
|
||
return self._kicker_velocity.refresh().value_as_double
|
||
|
||
# ── MagicBot lifecycle ─────────────────────────────────────────────────────
|
||
|
||
def execute(self) -> None:
|
||
# No periodic logic required; all control is driven by external callers.
|
||
pass
|