Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'kraken_swerve'

Browse Source 
release the kraken
This commit is contained in:
Tylr-J42 2025-01-27 21:38:03 -05:00
commit 0e91643b57
2 changed files with 79 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
src/main/java/frc/robot/constants/ModuleConstants.java
View File

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

44
src/main/java/frc/robot/subsystems/MAXSwerveModule.java
View File

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