diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index 1b3de28..a5f1f87 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -2,6 +2,12 @@ package frc.robot.constants; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.spark.config.SparkMaxConfig; public class ModuleConstants { @@ -20,52 +26,73 @@ public class ModuleConstants { public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; - public static final int kDriveMotorCurrentLimit = 40; + public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction; + public static final double kTurningFactor = 2 * Math.PI; + public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps; + + public static final double kDriveP = .04; + public static final double kDriveI = 0; + public static final double kDriveD = 0; + public static final double kDriveS = 0; + public static final double kDriveV = kDrivingVelocityFeedForward; + public static final double kDriveA = 0; + + public static final double kTurnP = 1; + public static final double kTurnI = 0; + public static final double kTurnD = 0; + + public static final int kDriveMotorStatorCurrentLimit = 120; public static final int kTurnMotorCurrentLimit = 20; + public static final IdleMode kTurnIdleMode = IdleMode.kBrake; + + public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake; + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM - public static final SparkMaxConfig drivingConfig = new SparkMaxConfig(); public static final SparkMaxConfig turningConfig = new SparkMaxConfig(); - static { - // Use module constants to calculate conversion factors and feed forward gain. - double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction; - double turningFactor = 2 * Math.PI; - double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps; + public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs(); + public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs(); + public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs(); + public static final Slot0Configs kDriveSlot0Config = new Slot0Configs(); - drivingConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(kDriveMotorCurrentLimit); - drivingConfig.encoder - .positionConversionFactor(drivingFactor) // meters - .velocityConversionFactor(drivingFactor / 60.0); // meters per second - drivingConfig.closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - // These are example gains you may need to them for your own robot! - .pid(0.04, 0, 0) - .velocityFF(drivingVelocityFeedForward) - .outputRange(-1, 1); + static { + kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction; + + kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true; + kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit; + + kDriveMotorConfig.Inverted = kDriveInversionState; + kDriveMotorConfig.NeutralMode = kDriveIdleMode; + + kDriveSlot0Config.kP = kDriveP; + kDriveSlot0Config.kI = kDriveI; + kDriveSlot0Config.kD = kDriveD; + kDriveSlot0Config.kS = kDriveS; + kDriveSlot0Config.kV = kDriveV; + kDriveSlot0Config.kA = kDriveA; turningConfig - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(20); + .idleMode(kTurnIdleMode) + .smartCurrentLimit(kTurnMotorCurrentLimit); turningConfig.absoluteEncoder // Invert the turning encoder, since the output shaft rotates in the opposite // direction of the steering motor in the MAXSwerve Module. .inverted(true) - .positionConversionFactor(turningFactor) // radians - .velocityConversionFactor(turningFactor / 60.0); // radians per second + .positionConversionFactor(kTurningFactor) // radians + .velocityConversionFactor(kTurningFactor / 60.0); // radians per second turningConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) // These are example gains you may need to them for your own robot! - .pid(1, 0, 0) + .pid(kTurnP, kTurnI, kTurnD) .outputRange(-1, 1) // Enable PID wrap around for the turning motor. This will allow the PID // controller to go through 0 to get to the setpoint i.e. going from 350 degrees // to 10 degrees will go through 0 rather than the other direction which is a // longer route. .positionWrappingEnabled(true) - .positionWrappingInputRange(0, turningFactor); + .positionWrappingInputRange(0, kTurningFactor); } } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 0b37934..f5d868e 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -15,21 +15,22 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; import frc.robot.constants.ModuleConstants; public class MAXSwerveModule { - private final SparkMax m_drivingSpark; + private final TalonFX m_drive; private final SparkMax m_turningSpark; - private final RelativeEncoder m_drivingEncoder; private final AbsoluteEncoder m_turningEncoder; - private final SparkClosedLoopController m_drivingClosedLoopController; private final SparkClosedLoopController m_turningClosedLoopController; + private final VelocityVoltage driveVelocityRequest; + private double m_chassisAngularOffset = 0; private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d()); @@ -40,26 +41,29 @@ public class MAXSwerveModule { * Encoder. */ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { - m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless); + m_drive = new TalonFX(drivingCANId); m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); - m_drivingEncoder = m_drivingSpark.getEncoder(); m_turningEncoder = m_turningSpark.getAbsoluteEncoder(); - m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController(); m_turningClosedLoopController = m_turningSpark.getClosedLoopController(); + driveVelocityRequest = new VelocityVoltage(0).withSlot(0); + // Apply the respective configurations to the SPARKS. Reset parameters before // applying the configuration to bring the SPARK to a known good state. Persist // the settings to the SPARK to avoid losing them on a power cycle. - m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); + m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig); + m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config); + m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); m_chassisAngularOffset = chassisAngularOffset; m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition()); - m_drivingEncoder.setPosition(0); + m_drive.setPosition(0); } /** @@ -70,7 +74,7 @@ public class MAXSwerveModule { public SwerveModuleState getState() { // Apply chassis angular offset to the encoder position to get the position // relative to the chassis. - return new SwerveModuleState(m_drivingEncoder.getVelocity(), + return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); } @@ -82,8 +86,7 @@ public class MAXSwerveModule { public SwerveModulePosition getPosition() { // Apply chassis angular offset to the encoder position to get the position // relative to the chassis. - return new SwerveModulePosition( - m_drivingEncoder.getPosition(), + return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(), new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset)); } @@ -102,14 +105,21 @@ public class MAXSwerveModule { correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition())); // Command driving and turning SPARKS towards their respective setpoints. - m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity); + m_drive.setControl( + driveVelocityRequest.withVelocity( + correctedDesiredState.speedMetersPerSecond + ).withFeedForward( + correctedDesiredState.speedMetersPerSecond + ) + ); + m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition); m_desiredState = desiredState; } public void setVoltageDrive(double voltage){ - m_drivingSpark.setVoltage(voltage); + m_drive.setVoltage(voltage); } public void setVoltageTurn(double voltage) { @@ -117,7 +127,7 @@ public class MAXSwerveModule { } public double getVoltageDrive() { - return m_drivingSpark.get() * RobotController.getBatteryVoltage(); + return m_drive.get() * RobotController.getBatteryVoltage(); } public double getVoltageTurn() { @@ -126,6 +136,6 @@ public class MAXSwerveModule { /** Zeroes all the SwerveModule encoders. */ public void resetEncoders() { - m_drivingEncoder.setPosition(0); + m_drive.setPosition(0); } }