Skip to content

Commit 3dd7026

Browse files
dabeycorndabeycorn
committed
Created basic swerve drive skeleton
1 parent c08e9c7 commit 3dd7026

File tree

8 files changed

+352
-71
lines changed

8 files changed

+352
-71
lines changed

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIO;
1515
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIOSpark;
1616

17+
/** A class */
1718
public class Constants {
1819

1920
// * Select which robot to initalize
@@ -61,7 +62,6 @@ public static DeployMode getDeployMode() {
6162
}
6263

6364
// * SUBSYSTEM CONSTANTS
64-
6565
public class KDriveConstants {
6666
public static final Translation2d[] kDevModuleLocs = new Translation2d[] {};
6767

@@ -94,7 +94,7 @@ public class KDriveConstants {
9494
new ModuleIOSpark(kDevModuleConfigs[0])
9595
};
9696

97-
public static enum kDriveControlMode {
97+
public static enum DriveControlMode {
9898
OPENLOOP,
9999
CLOSEDLOOP
100100
}

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

Lines changed: 144 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,21 +6,162 @@
66

77
package org.frc6423.frc2025.subsystems.swerve;
88

9+
import edu.wpi.first.math.controller.PIDController;
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.geometry.Translation2d;
14+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
15+
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
16+
import edu.wpi.first.math.kinematics.SwerveModulePosition;
17+
import edu.wpi.first.math.kinematics.SwerveModuleState;
18+
import edu.wpi.first.wpilibj.DriverStation;
19+
import edu.wpi.first.wpilibj.DriverStation.Alliance;
20+
import edu.wpi.first.wpilibj2.command.Command;
21+
import edu.wpi.first.wpilibj2.command.Commands;
922
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1023
import java.util.Arrays;
24+
import java.util.function.DoubleSupplier;
25+
26+
import org.frc6423.frc2025.Robot;
27+
import org.frc6423.frc2025.Constants.KDriveConstants.DriveControlMode;
28+
import org.frc6423.frc2025.subsystems.swerve.gyro.GyroIO;
29+
import org.frc6423.frc2025.subsystems.swerve.gyro.GyroIOInputsAutoLogged;
30+
import org.frc6423.frc2025.subsystems.swerve.gyro.GyroIONavX;
1131
import org.frc6423.frc2025.subsystems.swerve.module.Module;
1232
import org.frc6423.frc2025.subsystems.swerve.module.ModuleIO;
33+
import org.littletonrobotics.junction.Logger;
1334

1435
public class SwerveSubsystem extends SubsystemBase {
1536

1637
private final Module[] m_modules;
1738

18-
public SwerveSubsystem(ModuleIO[] ios) {
39+
private final GyroIO m_gryo;
40+
private final GyroIOInputsAutoLogged m_gyroInputs;
41+
private Rotation2d m_simulationHeading;
42+
43+
private final SwerveDriveKinematics m_swerveKinematics;
44+
private final SwerveDrivePoseEstimator m_swerveOdometry;
45+
private ChassisSpeeds m_setpointVelocity;
46+
47+
private final PIDController m_rotationalVelocityFeedback;
48+
49+
public SwerveSubsystem(ModuleIO[] ios, Translation2d[] locs) {
50+
m_gryo = new GyroIONavX();
51+
m_gyroInputs = new GyroIOInputsAutoLogged();
52+
m_simulationHeading = new Rotation2d();
1953
m_modules = new Module[ios.length];
54+
for (int i = 0; i < ios.length; i++) {
55+
m_modules[i] = new Module(ios[i], i);
56+
}
2057

21-
Arrays.stream(ios).forEach((io) -> m_modules[0] = new Module(io, 0));
58+
m_swerveKinematics = new SwerveDriveKinematics(locs);
59+
m_swerveOdometry =
60+
new SwerveDrivePoseEstimator(
61+
m_swerveKinematics, new Rotation2d(), getModulePoses(), new Pose2d());
62+
63+
m_rotationalVelocityFeedback = new PIDController(
64+
0,
65+
0,
66+
0); // !
2267
}
2368

2469
@Override
25-
public void periodic() {}
70+
public void periodic() {
71+
// gyro update
72+
if (Robot.isReal()) m_gryo.updateInputs(m_gyroInputs);
73+
else m_simulationHeading.rotateBy(Rotation2d.fromRadians(getSetpointVelocity().omegaRadiansPerSecond));
74+
75+
Arrays.stream(m_modules).forEach(Module::periodic); // Update each module
76+
77+
Logger.processInputs("Swerve/Gyro", m_gyroInputs);
78+
Logger.recordOutput("Swerve/ModuleStates", getModuleStates());
79+
Logger.recordOutput("Swerve/DesiredVel", m_setpointVelocity);
80+
Logger.recordOutput("Swerve/Velocity", getVelocity());
81+
}
82+
83+
/**
84+
* Drives Robot based on input velocities
85+
*
86+
* @param velXMetersPerSec X translation velocity supplier
87+
* @param velYMetersPerSec Y translation velocity supplier
88+
* @param velOmegaRadsPerSec Omega rotation velocity supplier
89+
* @return {@link Command}
90+
*/
91+
public Command drive(DoubleSupplier velXMetersPerSec, DoubleSupplier velYMetersPerSec, DoubleSupplier velOmegaRadsPerSec) {
92+
return Commands.run(
93+
() -> setSetpointVelocities(
94+
ChassisSpeeds.fromFieldRelativeSpeeds(
95+
velXMetersPerSec.getAsDouble(),
96+
velYMetersPerSec.getAsDouble(),
97+
velOmegaRadsPerSec.getAsDouble(),
98+
DriverStation.getAlliance().get() == Alliance.Blue
99+
? getHeading()
100+
: getHeading().minus(Rotation2d.fromRotations(0.5))),
101+
DriveControlMode.OPENLOOP));
102+
}
103+
104+
/**
105+
* Drives Robot based on input velocities and desired heading
106+
*
107+
* @param velXMetersPerSec X translation velocity supplier
108+
* @param velYMetersPerSec Y translation velocity supplier
109+
* @param desiredHeading Desired robot orientation
110+
* @return {@link Command}
111+
*/
112+
public Command drive(DoubleSupplier velXMetersPerSec, DoubleSupplier velYMetersPerSec, Rotation2d desiredHeading) {
113+
return Commands.run(
114+
() -> setSetpointVelocities(
115+
ChassisSpeeds.fromFieldRelativeSpeeds(
116+
velXMetersPerSec.getAsDouble(),
117+
velYMetersPerSec.getAsDouble(),
118+
m_rotationalVelocityFeedback.calculate(getHeading().getRadians(), desiredHeading.getRadians()),
119+
getHeading()),
120+
DriveControlMode.OPENLOOP));
121+
}
122+
123+
/** Set setpoint velocities */
124+
public void setSetpointVelocities(ChassisSpeeds desiredSpeeds, DriveControlMode controlMode) {
125+
m_setpointVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, getHeading()); // converts to field relative speeds
126+
setModuleStates(
127+
m_swerveKinematics.toSwerveModuleStates(
128+
ChassisSpeeds.discretize(m_setpointVelocity, 0.02)),
129+
controlMode
130+
);
131+
}
132+
133+
/** Set swerve module setpoints */
134+
public void setModuleStates(SwerveModuleState[] states, DriveControlMode mode) {
135+
Arrays.stream(m_modules)
136+
.forEach((m) -> m.setDesiredSate(states[m.getIndex()], mode));
137+
}
138+
139+
/** Returns desired chassis velocities */ // !
140+
public ChassisSpeeds getSetpointVelocity() {
141+
return new ChassisSpeeds();
142+
}
143+
144+
/** Get current velocity */
145+
public ChassisSpeeds getVelocity() {
146+
return ChassisSpeeds.fromRobotRelativeSpeeds(m_swerveKinematics.toChassisSpeeds(getModuleStates()), getHeading());
147+
}
148+
149+
/** Get current heading */
150+
public Rotation2d getHeading() {
151+
return Robot.isReal() ? m_gyroInputs.orientation.toRotation2d() : m_simulationHeading;
152+
}
153+
154+
/** Get Swerve Module Positions */
155+
public SwerveModulePosition[] getModulePoses() {
156+
return Arrays.stream(m_modules)
157+
.map(Module::getCurrentPose)
158+
.toArray(SwerveModulePosition[]::new);
159+
}
160+
161+
/** Get Swerve Module States */
162+
public SwerveModuleState[] getModuleStates() {
163+
return Arrays.stream(m_modules)
164+
.map(Module::getCurrentState)
165+
.toArray(SwerveModuleState[]::new);
166+
}
26167
}

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/gyro/GyroIO.java‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,15 @@
66

77
package org.frc6423.frc2025.subsystems.swerve.gyro;
88

9-
public class GyroIO {}
9+
import edu.wpi.first.math.geometry.Rotation3d;
10+
import org.littletonrobotics.junction.AutoLog;
11+
12+
public interface GyroIO {
13+
@AutoLog
14+
public class GyroIOInputs {
15+
public Rotation3d orientation = new Rotation3d();
16+
public double omegaRadsPerSec = 0.0;
17+
}
18+
19+
public void updateInputs(GyroIOInputs inputs);
20+
}

‎src/main/java/org/frc6423/frc2025/subsystems/swerve/gyro/GyroIONavX.java‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
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.subsystems.swerve.gyro;
8+
9+
import com.studica.frc.AHRS;
10+
import com.studica.frc.AHRS.NavXComType;
11+
import edu.wpi.first.math.util.Units;
12+
13+
public class GyroIONavX implements GyroIO {
14+
15+
private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI);
16+
17+
@Override
18+
public void updateInputs(GyroIOInputs inputs) {
19+
inputs.orientation = m_gyro.getRotation3d();
20+
inputs.omegaRadsPerSec = Units.degreesToRadians(m_gyro.getRate());
21+
}
22+
}

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

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

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

11+
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
1112
import edu.wpi.first.math.geometry.Rotation2d;
1213
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1314
import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -23,6 +24,8 @@ public class Module {
2324

2425
private SwerveModuleState m_goalState;
2526

27+
private final SimpleMotorFeedforward m_driveFeedforward;
28+
2629
private final Alert m_pivotConnectionAlert;
2730
private final Alert m_driveConnectionAlert;
2831

@@ -31,12 +34,15 @@ public Module(ModuleIO io, int index) {
3134
this.io = io;
3235
this.inputs = new ModuleIOInputsAutoLogged();
3336

37+
m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); // ! Add constants later
38+
3439
m_pivotConnectionAlert = new Alert(index + " pivot disconnected", AlertType.kWarning);
3540
m_driveConnectionAlert = new Alert(index + " drive disconnected", AlertType.kWarning);
3641
}
3742

3843
/** Update Module */
3944
public void periodic() {
45+
io.periodic();
4046
io.updateInputs(inputs);
4147
Logger.processInputs("Swerve/Module" + index, inputs);
4248

@@ -45,16 +51,23 @@ public void periodic() {
4551
}
4652

4753
/** Set swerve state goal */
48-
public void setDesiredSate(SwerveModuleState goalState) {
49-
m_goalState = goalState;
54+
public void setDesiredSate(SwerveModuleState goalState, DriveControlMode controlMode) {
5055
Rotation2d currentAngle = getCurrentState().angle;
5156

52-
m_goalState.optimize(currentAngle);
53-
// Decreases speed based on how far the module angle is from goal
54-
m_goalState.cosineScale(currentAngle);
57+
goalState.optimize(currentAngle);
58+
// Scaless speed based on how far the module angle is from goal angle
59+
goalState.speedMetersPerSecond *= goalState.angle.minus(currentAngle).getCos();
60+
61+
io.setPivotAngle(goalState.angle);
5562

56-
io.setPivotAngle(m_goalState.angle);
57-
io.setDriveVelocity(m_goalState.speedMetersPerSecond); // / kDriveReduction);
63+
if (controlMode == DriveControlMode.CLOSEDLOOP) {
64+
io.setDriveVelocity(
65+
goalState.speedMetersPerSecond,
66+
m_driveFeedforward.calculate(goalState.speedMetersPerSecond)); // Closed loop control
67+
} else {
68+
io.setDriveVolts(
69+
m_driveFeedforward.calculate(goalState.speedMetersPerSecond)); // Open Loop control
70+
}
5871
}
5972

6073
/** Enable or Disable module coast */
@@ -82,4 +95,9 @@ public SwerveModuleState getCurrentState() {
8295
public SwerveModulePosition getCurrentPose() {
8396
return new SwerveModulePosition(inputs.drivePose, inputs.pivotPose);
8497
}
98+
99+
/** Get module index */
100+
public int getIndex() {
101+
return index;
102+
}
85103
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ public class ModuleIOInputs {
4646
public void setPivotAngle(Rotation2d angle);
4747

4848
/** Set Module velocity goal */
49-
public void setDriveVelocity(double velMetersPerSec);
49+
public void setDriveVelocity(double velMetersPerSec, double ff);
5050

5151
/** Enable or Disable module coast (Pivot) */
5252
public void setPivotCoastMode(boolean enabled);

0 commit comments

Comments
 (0)