Skip to content

Commit 5c8349b

Browse files
dabeycorndabeycorn
committed
Create ModuleConfig & SwerveConfig util classes
1 parent 8626240 commit 5c8349b

File tree

8 files changed

+167
-99
lines changed

8 files changed

+167
-99
lines changed

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

Lines changed: 28 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,10 @@
66

77
package org.frc6423.frc2025;
88

9+
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig;
10+
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig.moduleType;
11+
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
12+
913
import edu.wpi.first.math.geometry.Rotation2d;
1014
import edu.wpi.first.math.geometry.Translation2d;
1115
import edu.wpi.first.math.util.Units;
@@ -61,6 +65,30 @@ public static DeployMode getDeployMode() {
6165

6266
// * SUBSYSTEM CONSTANTS
6367
public class KDriveConstants {
68+
// Swerve Configs
69+
public static final SwerveConfig kDevBotConfig = new SwerveConfig(
70+
new ModuleConfig[] {
71+
new ModuleConfig(1, moduleType.SPARKMAX, 1, 2, 0, Rotation2d.fromRadians(0), true),
72+
new ModuleConfig(2, moduleType.SPARKMAX, 3, 4, 1, Rotation2d.fromRadians(0), true),
73+
new ModuleConfig(3, moduleType.SPARKMAX, 5, 6, 2, Rotation2d.fromRadians(0), true),
74+
new ModuleConfig(4, moduleType.SPARKMAX, 7, 8, 3, Rotation2d.fromRadians(0), true)
75+
},
76+
new Translation2d[] {
77+
new Translation2d(0.381, 0.381),
78+
new Translation2d(0.381, -0.381),
79+
new Translation2d(-0.381, 0.381),
80+
new Translation2d(-0.381, -0.381)
81+
},
82+
23.47,
83+
23.47,
84+
2,
85+
2,
86+
56, // !
87+
Units.feetToMeters(16),
88+
Units.feetToMeters(16),
89+
100
90+
);
91+
6492
// Swerve Constants
6593
public static final double kDriveBaseWidth = Units.inchesToMeters(25.0);
6694
public static final double kDriveBaseLength = Units.inchesToMeters(25.0);
@@ -74,39 +102,6 @@ public class KDriveConstants {
74102
public static final double kSwerveRotationalI = 0.0;
75103
public static final double kSwerveRotationalD = 0.0;
76104

77-
// Module Constants
78-
public static final Translation2d[] kDevModuleLocs =
79-
new Translation2d[] {
80-
new Translation2d(0.381, 0.381),
81-
new Translation2d(0.381, -0.381),
82-
new Translation2d(-0.381, 0.381),
83-
new Translation2d(-0.381, -0.381)
84-
};
85-
86-
public static final Translation2d[] kCompModuleLocs =
87-
new Translation2d[] {
88-
new Translation2d(0.381, 0.381),
89-
new Translation2d(0.381, -0.381),
90-
new Translation2d(-0.381, 0.381),
91-
new Translation2d(-0.381, -0.381)
92-
};
93-
94-
public static ModuleConfig[] kDevBotConfigs =
95-
new ModuleConfig[] {
96-
new ModuleConfig(1, 1, 2, 0, Rotation2d.fromRadians(0), true),
97-
new ModuleConfig(2, 3, 4, 1, Rotation2d.fromRadians(0), true),
98-
new ModuleConfig(3, 5, 6, 2, Rotation2d.fromRadians(0), true),
99-
new ModuleConfig(4, 7, 8, 3, Rotation2d.fromRadians(0), true)
100-
};
101-
102-
public static ModuleConfig[] kCompBotConfigs =
103-
new ModuleConfig[] {
104-
new ModuleConfig(1, 1, 2, 9, Rotation2d.fromRadians(0), true),
105-
new ModuleConfig(2, 3, 4, 10, Rotation2d.fromRadians(0), true),
106-
new ModuleConfig(3, 5, 6, 11, Rotation2d.fromRadians(0), true),
107-
new ModuleConfig(4, 7, 8, 12, Rotation2d.fromRadians(0), true)
108-
};
109-
110105
// Global Module Constants (Same for all modules)
111106
public static enum DriveControlMode {
112107
OPENLOOP,
@@ -128,13 +123,5 @@ public static enum DriveControlMode {
128123
public static final double kDriveP = 1;
129124
public static final double kDriveI = 0;
130125
public static final double kDriveD = 0;
131-
132-
public record ModuleConfig(
133-
int index,
134-
int pivotID,
135-
int driveID,
136-
int pivotABSID,
137-
Rotation2d pivotOffset,
138-
boolean inverted) {}
139126
}
140127
}

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

Lines changed: 0 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -7,20 +7,11 @@
77
package org.frc6423.frc2025;
88

99
import static org.frc6423.frc2025.Constants.*;
10-
import static org.frc6423.frc2025.Constants.KDriveConstants.kMaxLinearSpeed;
11-
import static org.frc6423.frc2025.util.ControllerUtil.*;
1210

13-
import edu.wpi.first.math.geometry.Translation2d;
1411
import edu.wpi.first.wpilibj.RobotController;
1512
import edu.wpi.first.wpilibj.Threads;
1613
import edu.wpi.first.wpilibj.XboxController;
1714
import edu.wpi.first.wpilibj2.command.CommandScheduler;
18-
import java.util.Arrays;
19-
import org.frc6423.frc2025.Constants.KDriveConstants;
20-
import org.frc6423.frc2025.subsystems.swerve.SwerveSubsystem;
21-
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIO;
22-
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIOSim;
23-
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIOSpark;
2415
import org.littletonrobotics.junction.LogFileUtil;
2516
import org.littletonrobotics.junction.LoggedRobot;
2617
import org.littletonrobotics.junction.Logger;
@@ -32,8 +23,6 @@ public class Robot extends LoggedRobot {
3223

3324
private final XboxController m_DriverController;
3425

35-
private final SwerveSubsystem m_swerveSubsystem;
36-
3726
public Robot() {
3827
// AKit init
3928
Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
@@ -69,41 +58,8 @@ public Robot() {
6958
m_DriverController = new XboxController(0);
7059

7160
// Subsystem init
72-
ModuleIO[] moduleIos;
73-
Translation2d[] moduleLocs;
74-
switch (getRobot()) {
75-
case SIMULATED:
76-
// Swerve init
77-
moduleLocs = KDriveConstants.kCompModuleLocs;
78-
moduleIos = new ModuleIO[KDriveConstants.kCompBotConfigs.length];
79-
Arrays.stream(KDriveConstants.kCompBotConfigs)
80-
.forEach((c) -> moduleIos[c.index() - 1] = new ModuleIOSim(c));
81-
m_swerveSubsystem = new SwerveSubsystem(moduleIos, moduleLocs);
82-
break;
83-
case DEVBOT:
84-
// Swerve init
85-
moduleLocs = KDriveConstants.kDevModuleLocs;
86-
moduleIos = new ModuleIO[KDriveConstants.kDevBotConfigs.length];
87-
Arrays.stream(KDriveConstants.kDevBotConfigs)
88-
.forEach((c) -> moduleIos[c.index() - 1] = new ModuleIOSpark(c));
89-
m_swerveSubsystem = new SwerveSubsystem(moduleIos, moduleLocs);
90-
break;
91-
default: // Compbot
92-
// Swerve init
93-
moduleLocs = KDriveConstants.kCompModuleLocs;
94-
moduleIos = new ModuleIO[KDriveConstants.kCompBotConfigs.length];
95-
Arrays.stream(KDriveConstants.kCompBotConfigs)
96-
.forEach((c) -> moduleIos[c.index() - 1] = new ModuleIOSpark(c));
97-
m_swerveSubsystem = new SwerveSubsystem(moduleIos, moduleLocs);
98-
break;
99-
}
10061

10162
// Default Commands
102-
m_swerveSubsystem.setDefaultCommand(
103-
m_swerveSubsystem.drive(
104-
applyDeadband(m_DriverController::getLeftY) * kMaxLinearSpeed,
105-
applyDeadband(m_DriverController::getLeftX) * kMaxLinearSpeed,
106-
applyDeadband(m_DriverController::getRightX) * kMaxLinearSpeed));
10763
}
10864

10965
@Override

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

Lines changed: 19 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import edu.wpi.first.math.kinematics.SwerveModuleState;
1818
import edu.wpi.first.wpilibj.DriverStation;
1919
import edu.wpi.first.wpilibj.DriverStation.Alliance;
20+
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
2021
import edu.wpi.first.wpilibj2.command.Command;
2122
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2223
import java.util.Arrays;
@@ -27,6 +28,9 @@
2728
import org.frc6423.frc2025.subsystems.swerve.gyro.GyroIONavX;
2829
import org.frc6423.frc2025.subsystems.swerve.module.Module;
2930
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIO;
31+
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIOSpark;
32+
import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
33+
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig.moduleType;
3034
import org.littletonrobotics.junction.Logger;
3135

3236
public class SwerveSubsystem extends SubsystemBase {
@@ -43,16 +47,24 @@ public class SwerveSubsystem extends SubsystemBase {
4347

4448
private final PIDController m_rotationalVelocityFeedback;
4549

46-
public SwerveSubsystem(ModuleIO[] ios, Translation2d[] locs) {
47-
m_gryo = new GyroIONavX();
50+
public SwerveSubsystem(SwerveConfig config) {
51+
m_gryo = config.kGyroID == 100 ? new GyroIONavX() : new GyroIONavX(); // ! Add pigeon gyro
4852
m_gyroInputs = new GyroIOInputsAutoLogged();
4953
m_simulationHeading = new Rotation2d();
50-
m_modules = new Module[ios.length];
51-
for (int i = 0; i < ios.length; i++) {
52-
m_modules[i] = new Module(ios[i], i);
53-
}
54+
m_modules = new Module[config.kModuleConfigs.length];
5455

55-
m_swerveKinematics = new SwerveDriveKinematics(locs);
56+
Arrays.stream(config.kModuleConfigs)
57+
.forEach((moduleConfig) -> {
58+
int index = moduleConfig.kIndex - 1;
59+
60+
m_modules[index] = new Module(
61+
(moduleConfig.kModuletype == moduleType.SPARKMAX)
62+
? new ModuleIOSpark(moduleConfig)
63+
: new ModuleIOSpark(moduleConfig), // ! Add talonfx module
64+
index);
65+
});
66+
67+
m_swerveKinematics = new SwerveDriveKinematics(config.kModuleLocs);
5668
m_swerveOdometry =
5769
new SwerveDrivePoseEstimator(
5870
m_swerveKinematics, new Rotation2d(), getModulePoses(), new Pose2d());

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

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,12 +8,13 @@
88

99
import static org.frc6423.frc2025.Constants.KDriveConstants.*;
1010

11+
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig;
12+
1113
import edu.wpi.first.math.controller.PIDController;
1214
import edu.wpi.first.math.geometry.Rotation2d;
1315
import edu.wpi.first.math.system.plant.DCMotor;
1416
import edu.wpi.first.math.system.plant.LinearSystemId;
1517
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
16-
import org.frc6423.frc2025.Constants.KDriveConstants.ModuleConfig;
1718

1819
public class ModuleIOSim implements ModuleIO {
1920

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

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88

99
import static org.frc6423.frc2025.Constants.KDriveConstants.*;
1010

11+
import org.frc6423.frc2025.util.swerveUtil.ModuleConfig;
12+
1113
import com.revrobotics.AbsoluteEncoder;
1214
import com.revrobotics.RelativeEncoder;
1315
import com.revrobotics.spark.ClosedLoopSlot;
@@ -22,7 +24,6 @@
2224
import com.revrobotics.spark.config.SparkMaxConfig;
2325
import edu.wpi.first.math.geometry.Rotation2d;
2426
import edu.wpi.first.math.util.Units;
25-
import org.frc6423.frc2025.Constants.KDriveConstants.ModuleConfig;
2627

2728
public class ModuleIOSpark implements ModuleIO {
2829

@@ -35,7 +36,7 @@ public class ModuleIOSpark implements ModuleIO {
3536

3637
public ModuleIOSpark(ModuleConfig config) {
3738
// Pivot init
38-
m_pivotMotor = new SparkMax(config.pivotID(), MotorType.kBrushless);
39+
m_pivotMotor = new SparkMax(config.kPivotID, MotorType.kBrushless);
3940
m_pivotRelativeEncoder = m_pivotMotor.getEncoder();
4041
m_pivotEncoder = m_pivotMotor.getAbsoluteEncoder();
4142

@@ -54,7 +55,7 @@ public ModuleIOSpark(ModuleConfig config) {
5455
.velocityConversionFactor(0.0) // ! Add conversion factors
5556
.uvwAverageDepth(2);
5657

57-
m_pivotConfig.absoluteEncoder.zeroOffset(config.pivotOffset().getRotations());
58+
m_pivotConfig.absoluteEncoder.zeroOffset(config.kPivotOffset.getRotations());
5859

5960
m_pivotConfig
6061
.closedLoop
@@ -67,7 +68,7 @@ public ModuleIOSpark(ModuleConfig config) {
6768
m_pivotConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
6869

6970
// Drive init
70-
m_driveMotor = new SparkMax(config.driveID(), MotorType.kBrushless);
71+
m_driveMotor = new SparkMax(config.kDriveID, MotorType.kBrushless);
7172
m_driveEncoder = m_driveMotor.getEncoder();
7273

7374
m_driveFeedback = m_driveMotor.getClosedLoopController();

‎src/main/java/org/frc6423/frc2025/util/ControllerUtil.java‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,8 @@
66

77
package org.frc6423.frc2025.util;
88

9-
import java.util.function.DoubleSupplier;
10-
119
import edu.wpi.first.math.MathUtil;
10+
import java.util.function.DoubleSupplier;
1211

1312
public class ControllerUtil {
1413
public static double applyDeadband(DoubleSupplier axis) {

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

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
// Copyright (c) 2025 FRC 6423 - Ward Melville Iron Patriots
2+
// https://github.com/FIRSTTeam6423
3+
//
4+
// Open Source Software; you can modify and/or share it under the terms of
5+
// MIT license file in the root directory of this project
6+
7+
package org.frc6423.frc2025.util.swerveUtil;
8+
9+
import edu.wpi.first.math.geometry.Rotation2d;
10+
11+
public class ModuleConfig {
12+
public int kIndex;
13+
public moduleType kModuletype;
14+
15+
public int kPivotID;
16+
public int kDriveID;
17+
public int kPivotABSID;
18+
19+
public Rotation2d kPivotOffset;
20+
public boolean kPivotInverted;
21+
22+
public static enum moduleType {
23+
SPARKMAX,
24+
TALONFX
25+
}
26+
27+
public ModuleConfig(
28+
int index,
29+
moduleType type,
30+
int pivotID,
31+
int driveID,
32+
int pivotABSID,
33+
Rotation2d pivotOffset,
34+
boolean pivotInverted) {
35+
this.kIndex = index;
36+
this.kModuletype = type;
37+
this.kPivotID = pivotID;
38+
this.kDriveID = driveID;
39+
this.kPivotABSID = pivotABSID;
40+
this.kPivotOffset = pivotOffset;
41+
this.kPivotInverted = pivotInverted;
42+
}
43+
}

0 commit comments

Comments
 (0)