Add simulated control for turret

components/turret.py

@@ -1,13 +1,75 @@
+import math
+
+from magicbot import tunable
+from phoenix6.configs import (
+    ClosedLoopGeneralConfigs,
+    ExternalFeedbackConfigs,
+    MotorOutputConfigs,
+    Slot0Configs,
+    TalonFXSConfiguration,
+)
+from phoenix6.controls import PositionVoltage
+from phoenix6.hardware import TalonFXS
+from phoenix6.signals import InvertedValue, NeutralModeValue
+from wpilib import DutyCycleEncoder, Mechanism2d, SmartDashboard
 from wpimath import units
 
+from ids import DioChannel, TalonId
+from utilities.rev import configure_through_bore_encoder
+
 
 class TurretComponent:
+    MOTOR_TO_TURRET_GEARING = 1 / (40 / 200)
+    TURRET_TO_ENCODER_GEARING = 1 / ((200 / 50) * (20 / 80))
+
+    desired_turret_angle = tunable(0.0)
+
     def __init__(self) -> None:
-        # TODO Implement this
         # Initialise Motor
+        self.motor = TalonFXS(TalonId.TURRET)
+        motor_config = self.motor.configurator
+        motor_config.apply(
+            TalonFXSConfiguration()
+            .with_motor_output(
+                MotorOutputConfigs()
+                .with_neutral_mode(NeutralModeValue.BRAKE)
+                .with_inverted(InvertedValue.COUNTER_CLOCKWISE_POSITIVE)
+            )
+            .with_closed_loop_general(
+                ClosedLoopGeneralConfigs().with_continuous_wrap(True)
+            )
+            .with_external_feedback(
+                ExternalFeedbackConfigs().with_sensor_to_mechanism_ratio(
+                    TurretComponent.MOTOR_TO_TURRET_GEARING
+                )
+            )
+            .with_slot0(Slot0Configs().with_k_p(0.1))
+        )
 
         # Initialise Encoder
-        pass
+        self.absolute_encoder = DutyCycleEncoder(DioChannel.TURRET_ENCODER)
+        configure_through_bore_encoder(self.absolute_encoder)
+        self.absolute_encoder.setInverted(False)
+
+        self.sim_mechanism = Mechanism2d(2, 2)
+        SmartDashboard.putData("Turret Sim", self.sim_mechanism)
+        root = self.sim_mechanism.getRoot("Turret", 1, 1)
+        self.sim_pointer = root.appendLigament("Pointer", length=0.75, angle=0)
+
+    def setup(self) -> None:
+        self._sync_turret()
+
+    def on_enable(self) -> None:
+        self._sync_turret()
+
+    def get_absolute_turret_azimuth(self):
+        return self.absolute_encoder.get() * TurretComponent.TURRET_TO_ENCODER_GEARING
+
+    def _sync_turret(self) -> None:
+        self.motor.set_position(self.get_absolute_turret_azimuth())
+
+    def current_azimuth(self) -> units.radians:
+        return self.motor.get_position().value * math.tau
 
     def slew_relative(self, angle: units.radians) -> None:
         # TODO Implement this
@@ -24,4 +86,7 @@ def execute(self) -> None:
         # wrap angle
 
         # run control cycle
-        pass
+        self.motor.set_control(PositionVoltage(self.desired_turret_angle))
+
+    def periodic(self) -> None:
+        self.sim_pointer.setAngle(math.degrees(self.current_azimuth()))

ids.py

@@ -19,6 +19,7 @@ class TalonId(enum.IntEnum):
 
     FLYWHEEL_LEFT = 9
     FLYWHEEL_RIGHT = 10
+    TURRET = 14
     FEEDER = 11
 
     LEFT_FUNNEL = 12
@@ -52,6 +53,8 @@ class DioChannel(enum.IntEnum):
 
     PORT_VISION_ENCODER = 0
 
+    TURRET_ENCODER = 1
+
 
 @enum.unique
 class PwmChannel(enum.IntEnum):

physics.py

@@ -10,6 +10,7 @@
 import wpilib
 from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionSystemSim
 from pyfrc.physics.core import PhysicsInterface
+from wpilib import DutyCycleEncoder
 from wpilib.simulation import (
     DCMotorSim,
     DutyCycleEncoderSim,
@@ -101,6 +102,46 @@ def update(self, dt: float) -> None:
             )
 
 
+class TalonFXSTurretSim:
+    def __init__(
+        self,
+        gearbox_motor: Callable[[int], DCMotor],
+        motor: phoenix6.hardware.TalonFXS,
+        # Reduction between motor and mechanism rotations, as output over input.
+        # If the mechanism spins slower than the motor, this number should be greater than one.
+        motor_gearing: float,
+        moi: kilogram_square_meters,
+        encoder: DutyCycleEncoder,
+        # Reduction between encoder and mechanism readings, as output over input.
+        # If the mechanism spins slower than the motor, this number should be greater than one.
+        encoder_gearing: float,
+    ):
+        gearbox = gearbox_motor(1)
+        self.plant = LinearSystemId.DCMotorSystem(gearbox, moi, motor_gearing)
+        self.motor_sim = DCMotorSim(self.plant, gearbox)
+        self.encoder_sim = DutyCycleEncoderSim(encoder)
+        self.motor_state = motor.sim_state
+        self.motor_gearing = motor_gearing
+        self.encoder_gearing = encoder_gearing
+
+    def update(self, dt: float):
+        # grab the motor voltage to propagate the velocity and position of the mechanism
+        voltage = self.motor_state.motor_voltage
+        self.motor_sim.setInputVoltage(voltage)
+        self.motor_sim.update(dt)
+
+        # back propagate to motor state
+        self.motor_state.set_raw_rotor_position(
+            self.motor_sim.getAngularPosition() * self.motor_gearing
+        )
+        self.motor_state.set_rotor_velocity(
+            self.motor_sim.getAngularVelocity() * self.motor_gearing
+        )
+
+        # forward propagate to external encoder
+        self.encoder_sim.set(self.motor_sim.getAngularPosition() * self.encoder_gearing)
+
+
 class SparkArmSim:
     def __init__(self, mech_sim: SingleJointedArmSim, motor_sim: rev.SparkSim) -> None:
         self.mech_sim = mech_sim
@@ -153,6 +194,15 @@ def __init__(self, physics_controller: PhysicsInterface, robot: MyRobot):
             for module in robot.chassis.modules
         ]
 
+        self.turret_sim = TalonFXSTurretSim(
+            DCMotor.minion,
+            robot.turret.motor,
+            robot.turret.MOTOR_TO_TURRET_GEARING,
+            0.0000001,
+            robot.turret.absolute_encoder,
+            robot.turret.TURRET_TO_ENCODER_GEARING,
+        )
+
         self.imu = robot.chassis.imu.sim_state
 
         self.vision_sim = VisionSystemSim("main")
@@ -191,6 +241,8 @@ def update_sim(self, now: float, tm_diff: float) -> None:
 
         self.physics_controller.drive(speeds, tm_diff)
 
+        self.turret_sim.update(tm_diff)
+
         self.port_vision_encoder_sim.set(
             constrain_angle(
                 (

robot.py

@@ -247,4 +247,7 @@ def disabledPeriodic(self) -> None:
 
     def robotPeriodic(self) -> None:
         super().robotPeriodic()
+
+        self.turret.periodic()
+
         self.port_vision._per_loop_cache.clear()