Skip to content

Commit 03f8883

Browse files
dabeycorndabeycorn
committed
Created basic swerve subsystem skeleton
1 parent 7b7681b commit 03f8883

File tree

5 files changed

+159
-5
lines changed

5 files changed

+159
-5
lines changed

‎src/main/java/org/frc6423/frc2025/Constants.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212
/** A class */
1313
public class Constants {
1414

15+
public static final double kTickSpeed = 0.02;
16+
1517
// * Select which robot to initalize
1618
private static final RobotType m_robotType = RobotType.SIMULATED;
1719

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java‎

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
package org.frc6423.frc2025.subsystems.swerve;
2+
3+
import static org.frc6423.frc2025.Constants.kTickSpeed;
4+
5+
import java.util.Arrays;
6+
7+
import org.frc6423.frc2025.subsystems.swerve.module.Module;
8+
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
9+
10+
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
11+
import edu.wpi.first.math.geometry.Pose2d;
12+
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
14+
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
15+
import edu.wpi.first.math.kinematics.SwerveModulePosition;
16+
import edu.wpi.first.math.kinematics.SwerveModuleState;
17+
import edu.wpi.first.math.system.plant.DCMotor;
18+
import edu.wpi.first.wpilibj.DriverStation;
19+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
20+
21+
public class SwerveSubsystem extends SubsystemBase {
22+
private final SwerveConfig m_config;
23+
24+
private final Module[] m_modules;
25+
26+
private SwerveDriveKinematics m_kinematics;
27+
private SwerveDrivePoseEstimator m_poseEstimator;
28+
29+
public SwerveSubsystem(SwerveConfig config) {
30+
// Create modules
31+
var moduleConfigs = config.getModuleConfigs();
32+
m_modules = new Module[moduleConfigs.length];
33+
Arrays.stream(moduleConfigs)
34+
.forEach((c) -> m_modules[c.kIndex] = new Module(c));
35+
36+
// Create math objects
37+
m_kinematics = new SwerveDriveKinematics(config.getModuleLocs());
38+
m_poseEstimator = new SwerveDrivePoseEstimator(m_kinematics, getHeading(), getModulePoses(), new Pose2d());
39+
40+
m_config = config;
41+
}
42+
43+
@Override
44+
public void periodic() {
45+
46+
// Stop module input when driverstation is disabled
47+
if (DriverStation.isDisabled()) {
48+
for (Module module : m_modules) {
49+
module.stop();
50+
}
51+
}
52+
}
53+
54+
/**
55+
* Runs swerve at desired robot relative velocity
56+
*
57+
* @param velocity desired velocity
58+
* @param openloopEnabled enable or disable open loop voltage control
59+
*/
60+
public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) {
61+
velocity = ChassisSpeeds.discretize(velocity, kTickSpeed);
62+
63+
SwerveModuleState[] desiredStates = m_kinematics.toSwerveModuleStates(velocity);
64+
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, m_config.getMaxLinearSpeedMetersPerSec());
65+
66+
for(int i = 0; i < desiredStates.length; i++) {
67+
if (openloopEnabled) {
68+
ChassisSpeeds currentVelocities = getVelocitiesRobotRelative();
69+
boolean focEnabled =
70+
Math.sqrt(
71+
Math.pow(currentVelocities.vxMetersPerSecond, 2)
72+
+ Math.pow(currentVelocities.vyMetersPerSecond, 2)) // converts linear velocity components to linear velocity
73+
< 1 * m_config.getMaxLinearSpeedMetersPerSec();
74+
75+
// Converts desired motor velocity into input voltage
76+
// OmegaRadsPerSec/(KvRadsPerVolt)
77+
double driveVoltage = desiredStates[i].speedMetersPerSecond / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt;
78+
m_modules[i].runSetpointOpenloop(driveVoltage, desiredStates[i].angle, focEnabled);
79+
} else {
80+
m_modules[i].runSetpoint(desiredStates[i]);
81+
}
82+
}
83+
}
84+
85+
/** Gets current robot velocity (robot relative) */
86+
public ChassisSpeeds getVelocitiesRobotRelative() {
87+
return m_kinematics.toChassisSpeeds(getModuleStates());
88+
}
89+
90+
/** Gets current robot heading */
91+
public Rotation2d getHeading() {
92+
return new Rotation2d();
93+
}
94+
95+
/** Gets current robot field pose */
96+
public Pose2d getPose() {
97+
return m_poseEstimator.getEstimatedPosition();
98+
}
99+
100+
/** Returns an array of module field positions */
101+
public SwerveModulePosition[] getModulePoses() {
102+
return Arrays.stream(m_modules)
103+
.map(Module::getModulePose)
104+
.toArray(SwerveModulePosition[]::new);
105+
}
106+
107+
/** Returns an array of module states */
108+
public SwerveModuleState[] getModuleStates() {
109+
return Arrays.stream(m_modules)
110+
.map(Module::getModuleState)
111+
.toArray(SwerveModuleState[]::new);
112+
}
113+
}

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
package org.frc6423.frc2025.subsystems.swerve.constants;
22

3+
import java.util.Arrays;
4+
35
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig;
46
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
57

@@ -94,7 +96,7 @@ public double getWheelRadiusInches() {
9496

9597
@Override
9698
public ModuleConfig[] getModuleConfigs() {
97-
return new ModuleConfig[] {
99+
var configs = new ModuleConfig[] {
98100
new ModuleConfig(
99101
1,
100102
1,
@@ -132,6 +134,11 @@ public ModuleConfig[] getModuleConfigs() {
132134
getPivotConfigSparkMax(),
133135
getDriveConfigSparkMax()),
134136
};
137+
138+
Arrays.stream(configs)
139+
.forEach((c) -> c.kWheelRadiusMeters = Units.inchesToMeters(getWheelRadiusInches())); // Shut up
140+
141+
return configs;
135142
}
136143

137144
// Gains

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java‎

Lines changed: 33 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,9 @@
55

66
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
77
import edu.wpi.first.math.geometry.Rotation2d;
8+
import edu.wpi.first.math.kinematics.SwerveModulePosition;
89
import edu.wpi.first.math.kinematics.SwerveModuleState;
10+
import edu.wpi.first.math.util.Units;
911

1012
public class Module {
1113
private final ModuleIO m_IO;
@@ -35,13 +37,13 @@ public void updateInputs() {
3537

3638
/** Periodically ran logic */
3739
public void periodic() {
38-
Logger.processInputs("Swerve/Module" + m_index, m_inputs);
40+
Logger.processInputs("Swerve/Module" + getModuleIndex(), m_inputs);
3941
}
4042

4143
/** Run SwerveModuleState setpoint */
4244
public SwerveModuleState runSetpoint(SwerveModuleState setpointState) {
4345
setpointState.optimize(getPivotAngle());
44-
setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos();
46+
setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians());
4547

4648
double speedMPS = setpointState.speedMetersPerSecond;
4749
m_IO.setPivotAngle(setpointState.angle);
@@ -56,9 +58,11 @@ public SwerveModuleState runSetpoint(SwerveModuleState setpointState, double dri
5658
}
5759

5860
/** Runs SwerveModuleState setpoint but runs drive in open loop mode */
59-
public SwerveModuleState runSetpointOpenloop(SwerveModuleState setpointState, boolean FOCEnabled) {
61+
public SwerveModuleState runSetpointOpenloop(double voltage, Rotation2d angle, boolean FOCEnabled) {
62+
SwerveModuleState setpointState = new SwerveModuleState(voltage, angle);
63+
6064
setpointState.optimize(getPivotAngle());
61-
setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos();
65+
setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians());
6266

6367
double speedMPS = setpointState.speedMetersPerSecond;
6468
m_IO.setPivotAngle(setpointState.angle);
@@ -92,8 +96,33 @@ public int getModuleIndex() {
9296
return m_index;
9397
}
9498

99+
/** Get Module config */
100+
public ModuleConfig getModuleConfig() {
101+
return m_config;
102+
}
103+
104+
/** Gets Drive Speed in MPS */
105+
public double getVelMetersPerSec() {
106+
return m_inputs.driveVelRadsPerSec * m_config.kWheelRadiusMeters;
107+
}
108+
95109
/** returns current module angle */
96110
public Rotation2d getPivotAngle() {
97111
return m_inputs.pivotABSPose;
98112
}
113+
114+
/** Returns drive pose in meters */
115+
public double getPoseMeters() {
116+
return m_inputs.drivePoseRads * m_config.kWheelRadiusMeters;
117+
}
118+
119+
/** Returns swerve field pose */
120+
public SwerveModulePosition getModulePose() {
121+
return new SwerveModulePosition(getPoseMeters(), getPivotAngle());
122+
}
123+
124+
/** Returns Module state */
125+
public SwerveModuleState getModuleState() {
126+
return new SwerveModuleState(getVelMetersPerSec(), getPivotAngle());
127+
}
99128
}

‎src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import com.revrobotics.spark.config.SparkMaxConfig;
1313

1414
import edu.wpi.first.math.geometry.Rotation2d;
15+
import edu.wpi.first.math.util.Units;
1516

1617
public class ModuleConfig {
1718
public int kIndex;
@@ -29,6 +30,8 @@ public class ModuleConfig {
2930

3031
public SparkMaxConfig kPivotConfigSparkMax, kDriveConfigSparkMax;
3132

33+
public double kWheelRadiusMeters = Units.inchesToMeters(2);
34+
3235
/** Create a new TalonFX modul */
3336
public ModuleConfig(
3437
int index,

0 commit comments

Comments
 (0)