Skip to content

Commit

Permalink
Update SwerveWithPathPlanner for beta-5
Browse files Browse the repository at this point in the history
  • Loading branch information
bhall-ctre authored Dec 16, 2023
1 parent c4247bd commit 9bcb482
Showing 1 changed file with 15 additions and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -10,18 +10,26 @@
import frc.robot.CommandSwerveDrivetrain;

public class TunerConstants {
// Both sets of gains need to be tuned to your individual robot
// The steer motor uses MotionMagicVoltage control
// Both sets of gains need to be tuned to your individual robot.

// The steer motor uses any SwerveModule.SteerRequestType control request with the
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.05)
.withKS(0).withKV(1.5).withKA(0);
// When using closed-loop control, the drive motor uses:
// - VelocityVoltage, if DrivetrainConstants.SupportsPro is false (default)
// - VelocityTorqueCurrentFOC, if DrivetrainConstants.SupportsPro is true
// When using closed-loop control, the drive motor uses the control
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(3).withKI(0).withKD(0)
.withKS(0).withKV(0).withKA(0);

// The closed-loop output type to use for the steer motors;
// This affects the PID/FF gains for the steer motors
private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage;
// The closed-loop output type to use for the drive motors;
// This affects the PID/FF gains for the drive motors
private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage;

// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
private static final double kSlipCurrentA = 300.0;
Expand Down Expand Up @@ -61,6 +69,8 @@ public class TunerConstants {
.withSlipCurrent(kSlipCurrentA)
.withSteerMotorGains(steerGains)
.withDriveMotorGains(driveGains)
.withSteerMotorClosedLoopOutput(steerClosedLoopOutput)
.withDriveMotorClosedLoopOutput(driveClosedLoopOutput)
.withSpeedAt12VoltsMps(kSpeedAt12VoltsMps)
.withSteerInertia(kSteerInertia)
.withDriveInertia(kDriveInertia)
Expand Down

0 comments on commit 9bcb482

Please sign in to comment.