From cf1ac4287e2bc9ace39ff93d2506563f8953bb4c Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Tue, 27 Jan 2026 19:14:45 +1100 Subject: [PATCH 1/2] add initialisation for core hardware --- components/turret.py | 58 +++++++++++++++++++++++++++++++++++++++++--- ids.py | 3 +++ 2 files changed, 58 insertions(+), 3 deletions(-) diff --git a/components/turret.py b/components/turret.py index bd44180..e115c14 100644 --- a/components/turret.py +++ b/components/turret.py @@ -1,13 +1,65 @@ +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 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) + + 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 slew_relative(self, angle: units.radians) -> None: # TODO Implement this @@ -24,4 +76,4 @@ def execute(self) -> None: # wrap angle # run control cycle - pass + self.motor.set_control(PositionVoltage(self.desired_turret_angle)) diff --git a/ids.py b/ids.py index 76c2764..73ad180 100644 --- a/ids.py +++ b/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): From 17fb46104e183cbefbc3b1999708a0eee1604683 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Tue, 27 Jan 2026 19:31:36 +1100 Subject: [PATCH 2/2] add turret simulation elements --- components/turret.py | 15 ++++++++++++- physics.py | 52 ++++++++++++++++++++++++++++++++++++++++++++ robot.py | 3 +++ 3 files changed, 69 insertions(+), 1 deletion(-) diff --git a/components/turret.py b/components/turret.py index e115c14..ad2b173 100644 --- a/components/turret.py +++ b/components/turret.py @@ -1,3 +1,5 @@ +import math + from magicbot import tunable from phoenix6.configs import ( ClosedLoopGeneralConfigs, @@ -9,7 +11,7 @@ from phoenix6.controls import PositionVoltage from phoenix6.hardware import TalonFXS from phoenix6.signals import InvertedValue, NeutralModeValue -from wpilib import DutyCycleEncoder +from wpilib import DutyCycleEncoder, Mechanism2d, SmartDashboard from wpimath import units from ids import DioChannel, TalonId @@ -49,6 +51,11 @@ def __init__(self) -> None: 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() @@ -61,6 +68,9 @@ def get_absolute_turret_azimuth(self): 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 # TODO update setpoint @@ -77,3 +87,6 @@ def execute(self) -> None: # run control cycle self.motor.set_control(PositionVoltage(self.desired_turret_angle)) + + def periodic(self) -> None: + self.sim_pointer.setAngle(math.degrees(self.current_azimuth())) diff --git a/physics.py b/physics.py index 35d1409..d270c20 100644 --- a/physics.py +++ b/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( ( diff --git a/robot.py b/robot.py index 8bab46a..c8d9b1b 100644 --- a/robot.py +++ b/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()