Skip to content

Improved Swerve #26

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
merged 8 commits into from
Oct 3, 2025
Merged

Improved Swerve #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
fb362ef
set swerve to use velocity
ShmayaR Sep 28, 2025
3c3b589
it workssssssssss
ShmayaR Sep 28, 2025
ddebcde
removed feedforward gains
Strflightmight09 Oct 3, 2025
a5b5130
Merge branch 'main' into voltage-based-swerve
Strflightmight09 Oct 3, 2025
64b548f
Merge branch 'main' into voltage-based-swerve
ShmayaR Oct 3, 2025
84bb74f
Update MotorSubsystem.java
ShmayaR Oct 3, 2025
218c8f3
Merge branch 'voltage-based-swerve' of https://github.com/Programming…
ShmayaR Oct 3, 2025
c01495f
Merge branch 'main' into voltage-based-swerve
ShmayaR Oct 3, 2025
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
4 changes: 2 additions & 2 deletions src/main/java/frc/trigon/robot/commands/CommandConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,13 @@ public class CommandConstants {
AutonomousConstants.ROBOT_CONFIG.moduleLocations,
RobotContainer.SWERVE::getDriveWheelPositionsRadians,
() -> RobotContainer.SWERVE.getHeading().getRadians(),
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),
RobotContainer.SWERVE
),
CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand(
RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose,
Rotation2d.fromDegrees(0),
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null),
(omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),
RobotContainer.SWERVE
);

Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,6 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs
DISABLED_TRIGGER.onFalse(new InstantCommand(() -> {
setAllSubsystemsBrakeAsync(true);
IS_BRAKING = true;
RobotContainer.SWERVE.resetSetpoint();
}).ignoringDisable(true));
}

Expand Down
36 changes: 8 additions & 28 deletions src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import com.pathplanner.lib.util.DriveFeedforwards;
import com.pathplanner.lib.util.swerve.SwerveSetpoint;
import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation3d;
Expand All @@ -19,7 +18,6 @@
import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
import lib.hardware.RobotHardwareStats;
import lib.hardware.phoenix6.Phoenix6SignalThread;
import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro;
import lib.hardware.phoenix6.pigeon2.Pigeon2Signal;
Expand All @@ -33,10 +31,8 @@ public class Swerve extends MotorSubsystem {
private final Pigeon2Gyro gyro = SwerveConstants.GYRO;
private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES;
private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance();
private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(AutonomousConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND);
public Pose2d targetPathPlannerPose = new Pose2d();
public boolean isPathPlannerDriving = false;
private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules));

public Swerve() {
setName("Swerve");
Expand All @@ -53,7 +49,7 @@ public void setBrake(boolean brake) {
public void sysIDDrive(double targetCurrent) {
SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2));
for (int i = 0; i < 4; i++) {
swerveModules[i].setDriveMotorTargetCurrent(targetCurrent);
swerveModules[i].setDriveMotorTargetVoltage(targetCurrent);
swerveModules[i].setTargetAngle(a[i].angle);
}
}
Comment on lines 49 to 55
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

🛠️ Refactor suggestion | 🟠 Major

Rename parameter targetCurrent to reflect voltage-based control.

The method now uses setDriveMotorTargetVoltage, but the parameter is still named targetCurrent, which is misleading.

Apply this diff to rename the parameter:

     @Override
-    public void sysIDDrive(double targetCurrent) {
+    public void sysIDDrive(double targetVoltage) {
         SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2));
         for (int i = 0; i < 4; i++) {
-            swerveModules[i].setDriveMotorTargetVoltage(targetCurrent);
+            swerveModules[i].setDriveMotorTargetVoltage(targetVoltage);
             swerveModules[i].setTargetAngle(a[i].angle);
         }
     }
📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
public void sysIDDrive(double targetCurrent) {
SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2));
for (int i = 0; i < 4; i++) {
swerveModules[i].setDriveMotorTargetCurrent(targetCurrent);
swerveModules[i].setDriveMotorTargetVoltage(targetCurrent);
swerveModules[i].setTargetAngle(a[i].angle);
}
}
@Override
public void sysIDDrive(double targetVoltage) {
SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2));
for (int i = 0; i < 4; i++) {
swerveModules[i].setDriveMotorTargetVoltage(targetVoltage);
swerveModules[i].setTargetAngle(a[i].angle);
}
}
🤖 Prompt for AI Agents 
In src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java around lines 50
to 56, the method parameter name targetCurrent is misleading because the method
calls setDriveMotorTargetVoltage; rename the parameter to targetVoltage in the
method signature and replace all usages inside the method (and update any
references/call sites in the codebase) to use targetVoltage so the name
correctly reflects voltage-based control.

Expand Down Expand Up @@ -176,33 +172,22 @@ public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, b
* Used for PathPlanner because it generates setpoints automatically.
*
* @param targetSpeeds the pre generated robot relative target speeds
* @param feedforwards the feedforwards to use
*/
public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds, DriveFeedforwards feedforwards) {
public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds) {
// if (isStill(targetSpeeds)) {
// stop();
// return;
// }

final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds);
final double[] targetForcesNm;
if (feedforwards == null)
targetForcesNm = new double[]{0, 0, 0, 0};
else
targetForcesNm = feedforwards.linearForcesNewtons();

setTargetModuleStates(swerveModuleStates, targetForcesNm);
setTargetModuleStates(swerveModuleStates);
}

public Rotation2d getDriveRelativeAngle() {
final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation();
return Flippable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle;
}

public void resetSetpoint() {
previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules));
}

public void initializeDrive(boolean shouldUseClosedLoop) {
setClosedLoop(shouldUseClosedLoop);
resetRotationController();
Expand Down Expand Up @@ -279,23 +264,18 @@ void selfRelativeDrive(double xPower, double yPower, FlippableRotation2d targetA
* @param targetSpeeds the desired robot-relative targetSpeeds
*/
public void selfRelativeDrive(ChassisSpeeds targetSpeeds) {
previousSetpoint = setpointGenerator.generateSetpoint(
previousSetpoint,
targetSpeeds,
RobotHardwareStats.getPeriodicTimeSeconds()
);

// if (isStill(previousSetpoint.robotRelativeSpeeds())) {
// if (isStill(targetSpeeds) {
// stop();
// return;
// }

setTargetModuleStates(previousSetpoint.moduleStates(), previousSetpoint.feedforwards().linearForcesNewtons());
final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds);
setTargetModuleStates(swerveModuleStates);
}

private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates, double[] targetForcesNm) {
private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
for (int i = 0; i < swerveModules.length; i++)
swerveModules[i].setTargetState(swerveModuleStates[i], targetForcesNm[i]);
swerveModules[i].setTargetState(swerveModuleStates[i]);
}
Comment on lines +276 to 279
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🔴 Critical

Desaturate wheel speeds before commanding modules.

Pulling raw states from toSwerveModuleStates can exceed the module’s max velocity whenever translation and rotation are combined, so we need to clamp them prior to issuing targets or the real robot will saturate and fall short of requested motions. Please reintroduce desaturation inside setTargetModuleStates.

-    private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
-        for (int i = 0; i < swerveModules.length; i++)
-            swerveModules[i].setTargetState(swerveModuleStates[i]);
+    private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+                swerveModuleStates,
+                AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS
+        );
+        for (int i = 0; i < swerveModules.length; i++)
+            swerveModules[i].setTargetState(swerveModuleStates[i]);
     }

Add the corresponding import edu.wpi.first.math.kinematics.SwerveDriveKinematics;.

📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
for (int i = 0; i < swerveModules.length; i++)
swerveModules[i].setTargetState(swerveModuleStates[i], targetForcesNm[i]);
swerveModules[i].setTargetState(swerveModuleStates[i]);
}
private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) {
// Clamp wheel speeds to the robot’s max before commanding
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates,
AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS
);
for (int i = 0; i < swerveModules.length; i++)
swerveModules[i].setTargetState(swerveModuleStates[i]);
}


private void setClosedLoop(boolean shouldUseClosedLoop) {
Expand Down
25 changes: 10 additions & 15 deletions src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
package frc.trigon.robot.subsystems.swerve.swervemodule;

import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.TorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
Expand All @@ -25,9 +24,8 @@ public class SwerveModule {
private final CANcoderEncoder steerEncoder;
private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC);
private final double wheelDiameter;
private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0).withUpdateFreqHz(1000);
private final VelocityVoltage driveVelocityRequest = new VelocityVoltage(0).withUpdateFreqHz(1000);
private final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC);
private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0);
private boolean shouldDriveMotorUseClosedLoop = true;
private SwerveModuleState targetState = new SwerveModuleState();
private double[]
Expand All @@ -50,15 +48,14 @@ public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter)
configureHardware(offsetRotations);
}

public void setTargetState(SwerveModuleState targetState, double targetForceNm) {
public void setTargetState(SwerveModuleState targetState) {
if (willOptimize(targetState)) {
targetState.optimize(getCurrentAngle());
targetForceNm *= -1;
}

this.targetState = targetState;
setTargetAngle(targetState.angle);
setTargetVelocity(targetState.speedMetersPerSecond, targetForceNm);
setTargetVelocity(targetState.speedMetersPerSecond);
}

public void setBrake(boolean brake) {
Expand Down Expand Up @@ -96,8 +93,8 @@ public void shouldDriveMotorUseClosedLoop(boolean shouldDriveMotorUseClosedLoop)
this.shouldDriveMotorUseClosedLoop = shouldDriveMotorUseClosedLoop;
}

public void setDriveMotorTargetCurrent(double targetCurrent) {
driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent));
public void setDriveMotorTargetVoltage(double targetVoltage) {
driveMotor.setControl(driveVoltageRequest.withOutput(targetVoltage));
}

public void setTargetAngle(Rotation2d angle) {
Expand Down Expand Up @@ -145,21 +142,19 @@ private boolean willOptimize(SwerveModuleState state) {
* The target velocity is set using either closed loop or open loop depending on {@link this#shouldDriveMotorUseClosedLoop}.
*
* @param targetVelocityMetersPerSecond the target velocity, in meters per second
* @param targetForceNm the target force of the module in newton meters
*/
private void setTargetVelocity(double targetVelocityMetersPerSecond, double targetForceNm) {
private void setTargetVelocity(double targetVelocityMetersPerSecond) {
if (shouldDriveMotorUseClosedLoop) {
setTargetClosedLoopVelocity(targetVelocityMetersPerSecond, targetForceNm);
setTargetClosedLoopVelocity(targetVelocityMetersPerSecond);
return;
}

setTargetOpenLoopVelocity(targetVelocityMetersPerSecond);
}

private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond, double targetForceNm) {
private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond) {
final double targetVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond);
final double targetAccelerationRotationsPerSecondSquared = metersToDriveWheelRotations(targetForceNm);
driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond).withAcceleration(targetAccelerationRotationsPerSecondSquared));
driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond));
}

private void setTargetOpenLoopVelocity(double targetVelocityMetersPerSecond) {
Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ static SimpleMotorSimulation createSteerMotorSimulation() {
return new SimpleMotorSimulation(STEER_MOTOR_GEARBOX, REAR_STEER_MOTOR_GEAR_RATIO, STEER_MOMENT_OF_INERTIA);
}

static TalonFXConfiguration generateDriveMotorConfiguration() {
public static TalonFXConfiguration generateDriveMotorConfiguration() {
final TalonFXConfiguration config = new TalonFXConfiguration();

config.Audio.BeepOnBoot = false;
Expand All @@ -77,12 +77,12 @@ static TalonFXConfiguration generateDriveMotorConfiguration() {
config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1;
config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1;

config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 50;
config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 50;
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.4708 : 5.25;
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : DRIVE_MOTOR_GEAR_RATIO / (1 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp);
config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 5.25;
config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0;
config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0;

config.Feedback.VelocityFilterTimeConstant = 0;

Expand Down
Loading