Skip to content

Foc and cancoders #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Luddo183 merged 2 commits into from
Feb 1, 2026
Merged

Foc and cancoders #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a6a30ff
adding foc and cancoder code
Luddo183 Jan 26, 2026
2eecfd9
rotorToSensor added
Luddo183 Feb 1, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions src/main/java/org/team2342/lib/motors/smart/SmartMotorConfig.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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;
Expand Down Expand Up @@ -90,5 +96,34 @@ public enum ControlType {
PROFILED_VELOCITY,
}

public record FeedbackConfig(
FeedbackType type,
int encoderID,
double rotorToSensor,
double offsetRotations,
boolean inverted) {
public static FeedbackConfig internal() {
return new FeedbackConfig(FeedbackType.INTERNAL, -1, 1.0, 0.0, false);
}

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 rotorToSensor, double offsetRotations, boolean inverted) {
return new FeedbackConfig(
FeedbackType.FUSED, encoderID, rotorToSensor, offsetRotations, inverted);
}

public enum FeedbackType {
INTERNAL,
REMOTE,
FUSED
}
}

public record FollowerConfig(int canID, boolean inverted) {}
}
3 changes: 3 additions & 0 deletions src/main/java/org/team2342/lib/motors/smart/SmartMotorIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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) {}
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/org/team2342/lib/motors/smart/SmartMotorIOSim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand Down Expand Up @@ -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;
Expand Down
116 changes: 96 additions & 20 deletions src/main/java/org/team2342/lib/motors/smart/SmartMotorIOTalonFX.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -44,6 +49,9 @@ public class SmartMotorIOTalonFX implements SmartMotorIO {
private final StatusSignal<Voltage> leaderAppliedVolts;
private final StatusSignal<Current> leaderCurrent;

private CANcoder cancoder = null;
private StatusSignal<Angle> absolutePosition;

private final StatusSignal<Voltage>[] followersAppliedVolts;
private final StatusSignal<Current>[] followersCurrent;

Expand All @@ -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));
Expand All @@ -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,
() ->
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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;
Expand All @@ -209,4 +225,64 @@ 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.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
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.feedbackConfig.rotorToSensor();
talonConfig.Feedback.SensorToMechanismRatio = config.gearRatio;
}
}
}

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));
}
}
Loading