Skip to content

Commit

Permalink
fix: make brake mode executor work better
Browse files Browse the repository at this point in the history
  • Loading branch information
@mimizh2418
mimizh2418 committed Feb 1, 2025
1 parent 3fe6c35 commit 797bc41
Show file tree
Hide file tree
Showing 7 changed files with 67 additions and 198 deletions.
6 changes: 0 additions & 6 deletions .idea/inspectionProfiles/Project_Default.xml
View file

This file was deleted.

3 changes: 2 additions & 1 deletion src/main/java/org/team1540/robot2025/subsystems/drive/DrivetrainConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@

public class DrivetrainConstants {
public static final double ODOMETRY_FREQUENCY = 250.0;
public static final String CAN_BUS = TunerConstants.kCANBus.getName();

public static final double DRIVEBASE_RADIUS = Math.max(
Math.max(
Expand Down Expand Up @@ -50,7 +51,7 @@ public class DrivetrainConstants {
1),
getModuleTranslations());

public static final boolean OPTIMIZE_SETPOINTS = true;
public static final boolean OPTIMIZE_SETPOINTS = false;

public static Translation2d[] getModuleTranslations() {
return new Translation2d[] {
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/org/team1540/robot2025/subsystems/drive/ModuleIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,7 @@ default void setTurnPosition(Rotation2d rotation) {}

/** Sets the neutral mode of the drive motor */
default void setDriveBrakeMode(boolean enabled) {}

/** Sets the neutral mode of the turn motor */
default void setTurnBrakeMode(boolean enabled) {}
}
49 changes: 41 additions & 8 deletions src/main/java/org/team1540/robot2025/subsystems/drive/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,8 @@
import java.util.Queue;
import java.util.concurrent.Executor;
import java.util.concurrent.Executors;
import org.team1540.robot2025.generated.TunerConstants;
import org.team1540.robot2025.util.PhoenixUtil;
import org.team1540.robot2025.util.swerve.ModuleHW;
import org.team1540.robot2025.util.swerve.ModuleHWConfigs;

/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
Expand All @@ -40,6 +39,11 @@ public class ModuleIOTalonFX implements ModuleIO {

private final TalonFXConfiguration driveConfig;
private final TalonFXConfiguration turnConfig;
private final CANcoderConfiguration cancoderConfig;

private StatusCode lastDriveConfigStatus;
private StatusCode lastTurnConfigStatus;
private StatusCode lastCancoderConfigStatus;

// Drive motor control requests
private final VoltageOut driveVoltageReq = new VoltageOut(0);
Expand Down Expand Up @@ -81,13 +85,21 @@ public class ModuleIOTalonFX implements ModuleIO {
public ModuleIOTalonFX(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants) {
this.constants = constants;
ModuleHW hw = ModuleHW.fromModuleConstants(constants, TunerConstants.kCANBus.getName());
drive = hw.driveMotor();
turn = hw.turnMotor();
cancoder = hw.turnEncoder();
ModuleHWConfigs hw = ModuleHWConfigs.fromModuleConstants(constants);
drive = new TalonFX(constants.DriveMotorId, DrivetrainConstants.CAN_BUS);
turn = new TalonFX(constants.SteerMotorId, DrivetrainConstants.CAN_BUS);
cancoder = new CANcoder(constants.EncoderId, DrivetrainConstants.CAN_BUS);

driveConfig = hw.driveConfig();
turnConfig = hw.turnConfig();
cancoderConfig = hw.turnEncoderConfig();

lastDriveConfigStatus =
PhoenixUtil.tryUntilOk(5, () -> drive.getConfigurator().apply(driveConfig));
lastTurnConfigStatus =
PhoenixUtil.tryUntilOk(5, () -> turn.getConfigurator().apply(turnConfig));
lastCancoderConfigStatus =
PhoenixUtil.tryUntilOk(5, () -> cancoder.getConfigurator().apply(cancoderConfig));

// Create timestamp queue
timestampQueue = OdometryThread.getInstance().makeTimestampQueue();
Expand Down Expand Up @@ -122,7 +134,7 @@ public ModuleIOTalonFX(
turnAppliedVolts,
turnCurrent,
turnTemp);
ParentDevice.optimizeBusUtilizationForAll(drive, turn);
ParentDevice.optimizeBusUtilizationForAll(drive, turn, cancoder);
}

@Override
Expand Down Expand Up @@ -204,10 +216,31 @@ public void setTurnPosition(Rotation2d rotation) {

@Override
public void setDriveBrakeMode(boolean enabled) {
if ((driveConfig.MotorOutput.NeutralMode == NeutralModeValue.Brake) == enabled
&& lastDriveConfigStatus.isOK()) {
return;
}

brakeModeExecutor.execute(() -> {
synchronized (driveConfig) {
driveConfig.MotorOutput.NeutralMode = enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast;
PhoenixUtil.tryUntilOk(5, () -> drive.getConfigurator().apply(driveConfig));
lastDriveConfigStatus =
PhoenixUtil.tryUntilOk(5, () -> drive.getConfigurator().apply(driveConfig.MotorOutput, 0.25));
}
});
}

@Override
public void setTurnBrakeMode(boolean enabled) {
if ((turnConfig.MotorOutput.NeutralMode == NeutralModeValue.Brake) == enabled && lastTurnConfigStatus.isOK()) {
return;
}

brakeModeExecutor.execute(() -> {
synchronized (turnConfig) {
turnConfig.MotorOutput.NeutralMode = enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast;
lastTurnConfigStatus =
PhoenixUtil.tryUntilOk(5, () -> turn.getConfigurator().apply(turnConfig.MotorOutput, 0.25));
}
});
}
Expand Down
144 changes: 0 additions & 144 deletions src/main/java/org/team1540/robot2025/util/ClosedLoopConfig.java
View file

This file was deleted.

12 changes: 8 additions & 4 deletions src/main/java/org/team1540/robot2025/util/PhoenixUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,16 @@

public class PhoenixUtil {
/** Attempts to run the command until no error is produced. */
public static void tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
public static StatusCode tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
StatusCode error = StatusCode.OK;
for (int i = 0; i < maxAttempts; i++) {
var error = command.get();
if (error.isOK()) return;
error = command.get();
if (error.isOK()) return error;
}
DriverStation.reportWarning(
"Failed to run a command on a CTRE device after " + maxAttempts + " attempts.", true);
"Failed to run a command on a CTRE device after " + maxAttempts + " attempts:\n" + "\t"
+ error.getDescription(),
true);
return error;
}
}
48 changes: 13 additions & 35 deletions ...m1540/robot2025/util/swerve/ModuleHW.java → ...obot2025/util/swerve/ModuleHWConfigs.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,30 +2,19 @@

import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import org.team1540.robot2025.util.PhoenixUtil;

public record ModuleHW(
TalonFX driveMotor,
TalonFX turnMotor,
CANcoder turnEncoder,
TalonFXConfiguration driveConfig,
TalonFXConfiguration turnConfig,
CANcoderConfiguration turnEncoderConfig) {
public static ModuleHW fromModuleConstants(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants,
String canBus) {
TalonFX drive = new TalonFX(constants.DriveMotorId, canBus);
public record ModuleHWConfigs(
TalonFXConfiguration driveConfig, TalonFXConfiguration turnConfig, CANcoderConfiguration turnEncoderConfig) {
public static ModuleHWConfigs fromModuleConstants(
SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants) {
TalonFXConfiguration driveConfig = constants.DriveMotorInitialConfigs;

driveConfig.MotorOutput.Inverted = constants.DriveMotorInverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
Expand All @@ -41,19 +30,7 @@ public static ModuleHW fromModuleConstants(
driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
driveConfig.Slot0 = constants.DriveMotorGains;

PhoenixUtil.tryUntilOk(5, () -> drive.getConfigurator().apply(driveConfig));

CANcoder turnEncoder = new CANcoder(constants.EncoderId, canBus);
CANcoderConfiguration turnEncoderConfig = constants.EncoderInitialConfigs;
turnEncoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset;
turnEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
turnEncoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;

PhoenixUtil.tryUntilOk(5, () -> turnEncoder.getConfigurator().apply(turnEncoderConfig));

TalonFX turn = new TalonFX(constants.SteerMotorId, canBus);
TalonFXConfiguration turnConfig = constants.SteerMotorInitialConfigs;

turnConfig.MotorOutput.Inverted = constants.SteerMotorInverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
Expand All @@ -70,7 +47,6 @@ public static ModuleHW fromModuleConstants(
case RemoteCANdiPWM2 -> FeedbackSensorSourceValue.RemoteCANdiPWM2;
default -> throw new IllegalStateException(
"Invalid turn feedback sensor source" + constants.FeedbackSource);};

turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId;
turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
turnConfig.Feedback.SensorToMechanismRatio = 1.0;
Expand All @@ -79,17 +55,19 @@ public static ModuleHW fromModuleConstants(
turnConfig.CurrentLimits.SupplyCurrentLowerTime = 0.5;
turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
turnConfig.Slot0 = constants.SteerMotorGains;
turnConfig.MotionMagic.MotionMagicCruiseVelocity =
Units.radiansToRotations(DCMotor.getFalcon500Foc(1).freeSpeedRadPerSec) / constants.SteerMotorGearRatio;
turnConfig.MotionMagic.MotionMagicCruiseVelocity = Units.radiansToRotations(
DCMotor.getFalcon500Foc(1).withReduction(constants.SteerMotorGearRatio).freeSpeedRadPerSec);
turnConfig.MotionMagic.MotionMagicAcceleration = turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.1;
turnConfig.MotionMagic.MotionMagicExpo_kV = constants.SteerMotorGearRatio
* Units.radiansToRotations(DCMotor.getFalcon500Foc(1).freeSpeedRadPerSec)
/ 12.0;
turnConfig.MotionMagic.MotionMagicExpo_kV = Units.radiansToRotations(
DCMotor.getFalcon500Foc(1).withReduction(constants.SteerMotorGearRatio).KvRadPerSecPerVolt);
turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1;
turnConfig.ClosedLoopGeneral.ContinuousWrap = true;

PhoenixUtil.tryUntilOk(5, () -> turn.getConfigurator().apply(turnConfig));
CANcoderConfiguration turnEncoderConfig = constants.EncoderInitialConfigs;
turnEncoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset;
turnEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
turnEncoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;

return new ModuleHW(drive, turn, turnEncoder, driveConfig, turnConfig, turnEncoderConfig);
return new ModuleHWConfigs(driveConfig, turnConfig, turnEncoderConfig);
}
}

0 comments on commit 797bc41

Please sign in to comment.