From a6a30ff39f7413a6dfe2fb3266b0b94a526db29e Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Sun, 25 Jan 2026 23:15:17 -0500 Subject: [PATCH 1/2] adding foc and cancoder code --- .../lib/motors/smart/SmartMotorConfig.java | 27 ++ .../lib/motors/smart/SmartMotorIO.java | 3 + .../lib/motors/smart/SmartMotorIOSim.java | 6 + .../lib/motors/smart/SmartMotorIOTalonFX.java | 115 +++++-- .../motors/smart/SmartMotorIOTalonFXFOC.java | 289 ++++++++++++++++++ 5 files changed, 420 insertions(+), 20 deletions(-) create mode 100644 src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java index 89b3a3c..1dfb8c0 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java @@ -15,6 +15,7 @@ public class SmartMotorConfig extends MotorConfig { public PIDFFConfigs pidffConfigs = new PIDFFConfigs(); public double gearRatio = 1; public ControlType controlType = null; + public FeedbackConfig feedbackConfig = FeedbackConfig.internal(); public Constraints profileConstraintsRad = new Constraints(0, 0); @@ -41,6 +42,11 @@ public SmartMotorConfig withControlType(ControlType type) { return this; } + public SmartMotorConfig withFeedbackConfig(FeedbackConfig feedbackConfig) { + this.feedbackConfig = feedbackConfig; + return this; + } + public SmartMotorConfig withProfileConstraintsRad(Constraints constraints) { this.profileConstraintsRad = constraints; return this; @@ -90,5 +96,26 @@ public enum ControlType { PROFILED_VELOCITY, } + public record FeedbackConfig( + FeedbackType type, int encoderID, double offsetRotations, boolean inverted) { + public static FeedbackConfig internal() { + return new FeedbackConfig(FeedbackType.INTERNAL, -1, 0.0, false); + } + + public static FeedbackConfig remote(int encoderID, double offsetRotations, boolean inverted) { + return new FeedbackConfig(FeedbackType.REMOTE, encoderID, offsetRotations, inverted); + } + + public static FeedbackConfig fused(int encoderID, double offsetRotations, boolean inverted) { + return new FeedbackConfig(FeedbackType.FUSED, encoderID, offsetRotations, inverted); + } + + public enum FeedbackType { + INTERNAL, + REMOTE, + FUSED + } + } + public record FollowerConfig(int canID, boolean inverted) {} } diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIO.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIO.java index 70540f7..72f6e23 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIO.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIO.java @@ -15,6 +15,7 @@ public interface SmartMotorIO { public class SmartMotorIOInputs { public boolean[] motorsConnected; public double positionRad = 0.0; + public double absEncoderPositionRad = 0.0; public double velocityRadPerSec = 0.0; public double[] appliedVolts = new double[] {0.0}; public double[] currentAmps = new double[] {0.0}; @@ -28,6 +29,8 @@ public default void runVelocity(double velocityRadPerSec) {} public default void runVoltage(double voltage) {} + public default void runTorqueCurrent(double amps) {} + public default void reconfigurePIDFF(PIDFFConfigs configs) {} public default void setPosition(double positionRad) {} diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOSim.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOSim.java index ff7eb74..c353e51 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOSim.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOSim.java @@ -81,6 +81,7 @@ public void updateInputs(SmartMotorIOInputs inputs) { inputs.currentAmps = new double[followers]; inputs.positionRad = (sim.getOutput(0) / config.simRatio) + offset; + inputs.absEncoderPositionRad = sim.getOutput(0) + offset; inputs.velocityRadPerSec = sim.getOutput(1) / config.simRatio; for (int i = 0; i < followers + 1; i++) { @@ -140,6 +141,11 @@ public void runVoltage(double voltage) { sim.setInput(voltage); } + @Override + public void runTorqueCurrent(double amps) { + runVoltage(motor.getVoltage(motor.getTorque(amps), sim.getOutput(1) / config.simRatio)); + } + @Override public void reconfigurePIDFF(PIDFFConfigs configs) { this.config.pidffConfigs = configs; diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java index 297898b..d53f14b 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; @@ -16,11 +17,14 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; @@ -30,6 +34,7 @@ import org.team2342.frc.util.PhoenixUtils; import org.team2342.lib.motors.MotorConfig; import org.team2342.lib.motors.smart.SmartMotorConfig.ControlType; +import org.team2342.lib.motors.smart.SmartMotorConfig.FeedbackConfig; import org.team2342.lib.motors.smart.SmartMotorConfig.FollowerConfig; import org.team2342.lib.pidff.PIDFFConfigs; @@ -44,6 +49,9 @@ public class SmartMotorIOTalonFX implements SmartMotorIO { private final StatusSignal leaderAppliedVolts; private final StatusSignal leaderCurrent; + private CANcoder cancoder = null; + private StatusSignal absolutePosition; + private final StatusSignal[] followersAppliedVolts; private final StatusSignal[] followersCurrent; @@ -67,27 +75,15 @@ public SmartMotorIOTalonFX(int canID, SmartMotorConfig config, FollowerConfig... leaderTalon = new TalonFX(canID); - // Configure Drive talonConfig = new TalonFXConfiguration(); - talonConfig.MotorOutput.NeutralMode = - config.idleMode == MotorConfig.IdleMode.BRAKE - ? NeutralModeValue.Brake - : NeutralModeValue.Coast; - talonConfig.Slot0 = Slot0Configs.from(config.pidffConfigs.asPhoenixSlotConfigs()); - talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; - talonConfig.MotorOutput.Inverted = - config.motorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - talonConfig.CurrentLimits.StatorCurrentLimit = config.statorLimit; - talonConfig.CurrentLimits.StatorCurrentLimitEnable = config.statorLimit > 0; - talonConfig.CurrentLimits.SupplyCurrentLimit = config.supplyLimit; - talonConfig.CurrentLimits.SupplyCurrentLimitEnable = config.supplyLimit > 0; - - talonConfig.MotionMagic.MotionMagicCruiseVelocity = - Units.radiansToRotations(config.profileConstraintsRad.maxVelocity); - talonConfig.MotionMagic.MotionMagicAcceleration = - Units.radiansToRotations(config.profileConstraintsRad.maxAcceleration); + configureTalon(); + configureFeedback(); + if (cancoder != null) { + absolutePosition = cancoder.getAbsolutePosition(); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, absolutePosition); + PhoenixUtils.registerSignals(absolutePosition); + PhoenixUtils.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(cancoder)); + } PhoenixUtils.tryUntilOk(5, () -> leaderTalon.getConfigurator().apply(talonConfig, 0.25)); PhoenixUtils.tryUntilOk(5, () -> leaderTalon.setPosition(0, 0.25)); @@ -107,6 +103,16 @@ public SmartMotorIOTalonFX(int canID, SmartMotorConfig config, FollowerConfig... FollowerConfig followerConfig = followers[i]; followerTalons[i] = new TalonFX(followerConfig.canID()); final int j = i; + + var followerConfigFX = new TalonFXConfiguration(); + followerConfigFX.MotorOutput.NeutralMode = + config.idleMode == MotorConfig.IdleMode.BRAKE + ? NeutralModeValue.Brake + : NeutralModeValue.Coast; + + PhoenixUtils.tryUntilOk( + 5, () -> followerTalons[j].getConfigurator().apply(followerConfigFX, 0.25)); + PhoenixUtils.tryUntilOk( 5, () -> @@ -151,6 +157,10 @@ public void updateInputs(SmartMotorIOInputs inputs) { inputs.appliedVolts[0] = leaderAppliedVolts.getValueAsDouble(); inputs.currentAmps[0] = leaderCurrent.getValueAsDouble(); + if (cancoder != null) { + inputs.absEncoderPositionRad = Units.rotationsToRadians(absolutePosition.getValueAsDouble()); + } + for (int i = 0; i < followerTalons.length; i++) { inputs.motorsConnected[i + 1] = followersConnectedDebounce[i].calculate( @@ -197,6 +207,12 @@ public void runVoltage(double voltage) { leaderTalon.setControl(voltageRequest.withOutput(voltage)); } + @Override + public void runTorqueCurrent(double amps) { + throw new IllegalStateException( + "Cannot run torque control: Use a SmartMotorIOTalonFXFOC instead!"); + } + @Override public void reconfigurePIDFF(PIDFFConfigs configs) { this.config.pidffConfigs = configs; @@ -209,4 +225,63 @@ public void setPosition(double positionRad) { PhoenixUtils.tryUntilOk( 5, () -> leaderTalon.setPosition(Units.radiansToRotations(positionRad), 0.25)); } + + private void configureTalon() { + talonConfig.MotorOutput.NeutralMode = + config.idleMode == MotorConfig.IdleMode.BRAKE + ? NeutralModeValue.Brake + : NeutralModeValue.Coast; + talonConfig.Slot0 = Slot0Configs.from(config.pidffConfigs.asPhoenixSlotConfigs()); + talonConfig.MotorOutput.Inverted = + config.motorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + talonConfig.CurrentLimits.StatorCurrentLimit = config.statorLimit; + talonConfig.CurrentLimits.StatorCurrentLimitEnable = config.statorLimit > 0; + talonConfig.CurrentLimits.SupplyCurrentLimit = config.supplyLimit; + talonConfig.CurrentLimits.SupplyCurrentLimitEnable = config.supplyLimit > 0; + + talonConfig.MotionMagic.MotionMagicCruiseVelocity = + Units.radiansToRotations(config.profileConstraintsRad.maxVelocity); + talonConfig.MotionMagic.MotionMagicAcceleration = + Units.radiansToRotations(config.profileConstraintsRad.maxAcceleration); + } + + private void configureFeedback() { + switch (config.feedbackConfig.type()) { + case INTERNAL -> { + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; + } + + case REMOTE -> { + configureCANcoder(config.feedbackConfig); + + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; + } + + case FUSED -> { + configureCANcoder(config.feedbackConfig); + + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); + talonConfig.Feedback.RotorToSensorRatio = config.gearRatio; + talonConfig.Feedback.SensorToMechanismRatio = 1.0; + } + } + } + + private void configureCANcoder(FeedbackConfig feedback) { + cancoder = new CANcoder(feedback.encoderID()); + + var cfg = new CANcoderConfiguration(); + cfg.MagnetSensor.SensorDirection = + feedback.inverted() + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cfg.MagnetSensor.MagnetOffset = feedback.offsetRotations(); + + PhoenixUtils.tryUntilOk(5, () -> cancoder.getConfigurator().apply(cfg, 0.25)); + } } diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java new file mode 100644 index 0000000..264cab1 --- /dev/null +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java @@ -0,0 +1,289 @@ +// Copyright (c) 2026 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.lib.motors.smart; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; +import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import org.team2342.frc.util.PhoenixUtils; +import org.team2342.lib.motors.MotorConfig; +import org.team2342.lib.motors.smart.SmartMotorConfig.ControlType; +import org.team2342.lib.motors.smart.SmartMotorConfig.FeedbackConfig; +import org.team2342.lib.motors.smart.SmartMotorConfig.FollowerConfig; +import org.team2342.lib.pidff.PIDFFConfigs; + +public class SmartMotorIOTalonFXFOC implements SmartMotorIO { + + private final TalonFX leaderTalon; + private final TalonFX[] followerTalons; + + private final TalonFXConfiguration talonConfig; + private final StatusSignal leaderPosition; + private final StatusSignal leaderVelocity; + private final StatusSignal leaderAppliedVolts; + private final StatusSignal leaderCurrent; + + private CANcoder cancoder = null; + private StatusSignal absolutePosition; + + private final StatusSignal[] followersAppliedVolts; + private final StatusSignal[] followersCurrent; + + private final VoltageOut voltageRequest = new VoltageOut(0); + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + + private final VelocityTorqueCurrentFOC velocityRequest = new VelocityTorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionRequest = new PositionTorqueCurrentFOC(0); + + private final MotionMagicVelocityTorqueCurrentFOC velocityMotionMagicRequest = + new MotionMagicVelocityTorqueCurrentFOC(0); + private final MotionMagicTorqueCurrentFOC positionMotionMagicRequest = + new MotionMagicTorqueCurrentFOC(0); + + private final Debouncer leaderConnectedDebounce = new Debouncer(0.5); + private final Debouncer[] followersConnectedDebounce; + + private final SmartMotorConfig config; + + @SuppressWarnings("unchecked") // lol type "safety" + public SmartMotorIOTalonFXFOC(int canID, SmartMotorConfig config, FollowerConfig... followers) { + this.config = config; + + leaderTalon = new TalonFX(canID); + + talonConfig = new TalonFXConfiguration(); + configureTalon(); + configureFeedback(); + if (cancoder != null) { + absolutePosition = cancoder.getAbsolutePosition(); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, absolutePosition); + PhoenixUtils.registerSignals(absolutePosition); + PhoenixUtils.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(cancoder)); + } + + PhoenixUtils.tryUntilOk(5, () -> leaderTalon.getConfigurator().apply(talonConfig, 0.25)); + PhoenixUtils.tryUntilOk(5, () -> leaderTalon.setPosition(0, 0.25)); + + // Create input signals + leaderPosition = leaderTalon.getPosition(); + leaderVelocity = leaderTalon.getVelocity(); + leaderAppliedVolts = leaderTalon.getMotorVoltage(); + leaderCurrent = leaderTalon.getTorqueCurrent(); + + followerTalons = new TalonFX[followers.length]; + followersAppliedVolts = (StatusSignal[]) new StatusSignal[followers.length]; + followersCurrent = (StatusSignal[]) new StatusSignal[followers.length]; + followersConnectedDebounce = new Debouncer[followers.length]; + + for (int i = 0; i < followers.length; i++) { + FollowerConfig followerConfig = followers[i]; + followerTalons[i] = new TalonFX(followerConfig.canID()); + final int j = i; + + var followerConfigFX = new TalonFXConfiguration(); + followerConfigFX.MotorOutput.NeutralMode = + config.idleMode == MotorConfig.IdleMode.BRAKE + ? NeutralModeValue.Brake + : NeutralModeValue.Coast; + + PhoenixUtils.tryUntilOk( + 5, () -> followerTalons[j].getConfigurator().apply(followerConfigFX, 0.25)); + + PhoenixUtils.tryUntilOk( + 5, + () -> + followerTalons[j].setControl( + new Follower( + leaderTalon.getDeviceID(), + followerConfig.inverted() + ? MotorAlignmentValue.Opposed + : MotorAlignmentValue.Aligned))); + followersAppliedVolts[i] = followerTalons[i].getMotorVoltage(); + followersCurrent[i] = followerTalons[i].getTorqueCurrent(); + followersConnectedDebounce[i] = new Debouncer(0.5); + } + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, followersAppliedVolts); + BaseStatusSignal.setUpdateFrequencyForAll(50.0, followersCurrent); + + // Optimize utilization for everything + PhoenixUtils.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(leaderTalon)); + PhoenixUtils.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(followerTalons)); + + // Register signals for update + PhoenixUtils.registerSignals(leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent); + PhoenixUtils.registerSignals(followersAppliedVolts); + PhoenixUtils.registerSignals(followersCurrent); + } + + @Override + public void updateInputs(SmartMotorIOInputs inputs) { + inputs.motorsConnected = new boolean[1 + followerTalons.length]; + inputs.appliedVolts = new double[1 + followerTalons.length]; + inputs.currentAmps = new double[1 + followerTalons.length]; + + inputs.motorsConnected[0] = + leaderConnectedDebounce.calculate( + BaseStatusSignal.isAllGood( + leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent)); + inputs.positionRad = Units.rotationsToRadians(leaderPosition.getValueAsDouble()); + inputs.velocityRadPerSec = Units.rotationsToRadians(leaderVelocity.getValueAsDouble()); + inputs.appliedVolts[0] = leaderAppliedVolts.getValueAsDouble(); + inputs.currentAmps[0] = leaderCurrent.getValueAsDouble(); + + if (cancoder != null) { + inputs.absEncoderPositionRad = Units.rotationsToRadians(absolutePosition.getValueAsDouble()); + } + + for (int i = 0; i < followerTalons.length; i++) { + inputs.motorsConnected[i + 1] = + followersConnectedDebounce[i].calculate( + BaseStatusSignal.isAllGood(followersAppliedVolts[i], followersCurrent[i])); + inputs.appliedVolts[i + 1] = followersAppliedVolts[i].getValueAsDouble(); + inputs.currentAmps[i + 1] = followersCurrent[i].getValueAsDouble(); + } + } + + @Override + public void runVelocity(double velocityRadPerSec) { + if (config.controlType == ControlType.VELOCITY) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + leaderTalon.setControl(velocityRequest.withVelocity(velocityRotPerSec)); + } else if (config.controlType == ControlType.PROFILED_VELOCITY) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + leaderTalon.setControl(velocityMotionMagicRequest.withVelocity(velocityRotPerSec)); + } else { + throw new IllegalStateException( + "Cannot run velocity control: smart motor is configured for " + + config.controlType.toString() + + " control"); + } + } + + @Override + public void runPosition(double positionRad) { + if (config.controlType == ControlType.POSITION) { + double positionRot = Units.radiansToRotations(positionRad); + leaderTalon.setControl(positionRequest.withPosition(positionRot)); + } else if (config.controlType == ControlType.PROFILED_POSITION) { + double positionRot = Units.radiansToRotations(positionRad); + leaderTalon.setControl(positionMotionMagicRequest.withPosition(positionRot)); + } else { + throw new IllegalStateException( + "Cannot run position control: smart motor is configured for " + + config.controlType.toString() + + " control"); + } + } + + @Override + public void runVoltage(double voltage) { + leaderTalon.setControl(voltageRequest.withOutput(voltage)); + } + + @Override + public void runTorqueCurrent(double amps) { + leaderTalon.setControl(torqueCurrentRequest.withOutput(amps)); + } + + @Override + public void reconfigurePIDFF(PIDFFConfigs configs) { + this.config.pidffConfigs = configs; + talonConfig.Slot0 = Slot0Configs.from(config.pidffConfigs.asPhoenixSlotConfigs()); + PhoenixUtils.tryUntilOk(5, () -> leaderTalon.getConfigurator().apply(talonConfig, 0.25)); + } + + @Override + public void setPosition(double positionRad) { + PhoenixUtils.tryUntilOk( + 5, () -> leaderTalon.setPosition(Units.radiansToRotations(positionRad), 0.25)); + } + + private void configureTalon() { + talonConfig.MotorOutput.NeutralMode = + config.idleMode == MotorConfig.IdleMode.BRAKE + ? NeutralModeValue.Brake + : NeutralModeValue.Coast; + talonConfig.Slot0 = Slot0Configs.from(config.pidffConfigs.asPhoenixSlotConfigs()); + talonConfig.MotorOutput.Inverted = + config.motorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + talonConfig.CurrentLimits.StatorCurrentLimit = config.statorLimit; + talonConfig.CurrentLimits.StatorCurrentLimitEnable = config.statorLimit > 0; + talonConfig.CurrentLimits.SupplyCurrentLimit = config.supplyLimit; + talonConfig.CurrentLimits.SupplyCurrentLimitEnable = config.supplyLimit > 0; + + talonConfig.MotionMagic.MotionMagicCruiseVelocity = + Units.radiansToRotations(config.profileConstraintsRad.maxVelocity); + talonConfig.MotionMagic.MotionMagicAcceleration = + Units.radiansToRotations(config.profileConstraintsRad.maxAcceleration); + } + + private void configureFeedback() { + switch (config.feedbackConfig.type()) { + case INTERNAL -> { + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; + } + + case REMOTE -> { + configureCANcoder(config.feedbackConfig); + + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; + } + + case FUSED -> { + configureCANcoder(config.feedbackConfig); + + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); + talonConfig.Feedback.RotorToSensorRatio = config.gearRatio; + talonConfig.Feedback.SensorToMechanismRatio = 1.0; + } + } + } + + private void configureCANcoder(FeedbackConfig feedback) { + cancoder = new CANcoder(feedback.encoderID()); + + var cfg = new CANcoderConfiguration(); + cfg.MagnetSensor.SensorDirection = + feedback.inverted() + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cfg.MagnetSensor.MagnetOffset = feedback.offsetRotations(); + + PhoenixUtils.tryUntilOk(5, () -> cancoder.getConfigurator().apply(cfg, 0.25)); + } +} From 2eecfd989d485c29cc4611d8ef29729c5bea6cc9 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Sun, 1 Feb 2026 16:11:11 -0500 Subject: [PATCH 2/2] rotorToSensor added --- .../lib/motors/smart/SmartMotorConfig.java | 20 +++++++++++++------ .../lib/motors/smart/SmartMotorIOTalonFX.java | 5 +++-- .../motors/smart/SmartMotorIOTalonFXFOC.java | 5 +++-- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java index 1dfb8c0..7e94632 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java @@ -97,17 +97,25 @@ public enum ControlType { } public record FeedbackConfig( - FeedbackType type, int encoderID, double offsetRotations, boolean inverted) { + FeedbackType type, + int encoderID, + double rotorToSensor, + double offsetRotations, + boolean inverted) { public static FeedbackConfig internal() { - return new FeedbackConfig(FeedbackType.INTERNAL, -1, 0.0, false); + return new FeedbackConfig(FeedbackType.INTERNAL, -1, 1.0, 0.0, false); } - public static FeedbackConfig remote(int encoderID, double offsetRotations, boolean inverted) { - return new FeedbackConfig(FeedbackType.REMOTE, encoderID, offsetRotations, inverted); + public static FeedbackConfig remote( + int encoderID, double rotorToSensor, double offsetRotations, boolean inverted) { + return new FeedbackConfig( + FeedbackType.REMOTE, encoderID, rotorToSensor, offsetRotations, inverted); } - public static FeedbackConfig fused(int encoderID, double offsetRotations, boolean inverted) { - return new FeedbackConfig(FeedbackType.FUSED, encoderID, offsetRotations, inverted); + public static FeedbackConfig fused( + int encoderID, double rotorToSensor, double offsetRotations, boolean inverted) { + return new FeedbackConfig( + FeedbackType.FUSED, encoderID, rotorToSensor, offsetRotations, inverted); } public enum FeedbackType { diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java index d53f14b..80c296a 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java @@ -250,6 +250,7 @@ private void configureTalon() { private void configureFeedback() { switch (config.feedbackConfig.type()) { case INTERNAL -> { + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; } @@ -266,8 +267,8 @@ private void configureFeedback() { talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); - talonConfig.Feedback.RotorToSensorRatio = config.gearRatio; - talonConfig.Feedback.SensorToMechanismRatio = 1.0; + talonConfig.Feedback.RotorToSensorRatio = config.feedbackConfig.rotorToSensor(); + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; } } } diff --git a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java index 264cab1..83af716 100644 --- a/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java +++ b/src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFXFOC.java @@ -252,6 +252,7 @@ private void configureTalon() { private void configureFeedback() { switch (config.feedbackConfig.type()) { case INTERNAL -> { + talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; } @@ -268,8 +269,8 @@ private void configureFeedback() { talonConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; talonConfig.Feedback.FeedbackRemoteSensorID = config.feedbackConfig.encoderID(); - talonConfig.Feedback.RotorToSensorRatio = config.gearRatio; - talonConfig.Feedback.SensorToMechanismRatio = 1.0; + talonConfig.Feedback.RotorToSensorRatio = config.feedbackConfig.rotorToSensor(); + talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio; } } }