Skip to content
This repository was archived by the owner on Jun 20, 2025. It is now read-only.

Commit c88d0d9

Browse files
mimizh2418mimizh2418
committed
fix: passes checks (hopefully)
1 parent 630b56c commit c88d0d9

File tree

1 file changed

+192
-173
lines changed
  • src/main/java/frc/robot/subsystems/drive

1 file changed

+192
-173
lines changed

‎src/main/java/frc/robot/subsystems/drive/Drive.java

Lines changed: 192 additions & 173 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,6 @@
2626
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
2727
import edu.wpi.first.math.kinematics.SwerveModulePosition;
2828
import edu.wpi.first.math.kinematics.SwerveModuleState;
29-
import edu.wpi.first.math.util.Units;
3029
import edu.wpi.first.wpilibj.DriverStation;
3130
import edu.wpi.first.wpilibj2.command.SubsystemBase;
3231
import frc.robot.util.LocalADStarAK;
@@ -39,189 +38,209 @@
3938
public class Drive extends SubsystemBase {
4039

4140

42-
private final GyroIO gyroIO;
43-
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
44-
private final Module[] modules = new Module[4]; // FL, FR, BL, BR
45-
46-
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
47-
private Pose2d pose = new Pose2d();
48-
private Rotation2d lastGyroRotation = new Rotation2d();
49-
private boolean forceModuleAngleChange = false;
50-
51-
public Drive(
52-
GyroIO gyroIO,
53-
ModuleIO flModuleIO,
54-
ModuleIO frModuleIO,
55-
ModuleIO blModuleIO,
56-
ModuleIO brModuleIO) {
57-
this.gyroIO = gyroIO;
58-
modules[0] = new Module(flModuleIO, 0);
59-
modules[1] = new Module(frModuleIO, 1);
60-
modules[2] = new Module(blModuleIO, 2);
61-
modules[3] = new Module(brModuleIO, 3);
62-
63-
// Configure AutoBuilder for PathPlanner
64-
AutoBuilder.configureHolonomic(
65-
this::getPose,
66-
this::setPose,
67-
() -> kinematics.toChassisSpeeds(getModuleStates()),
68-
this::runVelocity,
69-
new HolonomicPathFollowerConfig(
70-
MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
71-
this);
72-
Pathfinding.setPathfinder(new LocalADStarAK());
73-
PathPlannerLogging.setLogActivePathCallback(
74-
(activePath) -> {
75-
Logger.recordOutput(
76-
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
77-
});
78-
PathPlannerLogging.setLogTargetPoseCallback(
79-
(targetPose) -> {
80-
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
81-
});
82-
}
83-
84-
public void periodic() {
85-
gyroIO.updateInputs(gyroInputs);
86-
Logger.processInputs("Drive/Gyro", gyroInputs);
87-
for (var module : modules) {
88-
module.periodic();
41+
private final GyroIO gyroIO;
42+
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
43+
private final Module[] modules = new Module[4]; // FL, FR, BL, BR
44+
45+
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
46+
private Pose2d pose = new Pose2d();
47+
private Rotation2d lastGyroRotation = new Rotation2d();
48+
private boolean forceModuleAngleChange = false;
49+
50+
public Drive(
51+
GyroIO gyroIO,
52+
ModuleIO flModuleIO,
53+
ModuleIO frModuleIO,
54+
ModuleIO blModuleIO,
55+
ModuleIO brModuleIO) {
56+
this.gyroIO = gyroIO;
57+
modules[0] = new Module(flModuleIO, 0);
58+
modules[1] = new Module(frModuleIO, 1);
59+
modules[2] = new Module(blModuleIO, 2);
60+
modules[3] = new Module(brModuleIO, 3);
61+
62+
// Configure AutoBuilder for PathPlanner
63+
AutoBuilder.configureHolonomic(
64+
this::getPose,
65+
this::setPose,
66+
() -> kinematics.toChassisSpeeds(getModuleStates()),
67+
this::runVelocity,
68+
new HolonomicPathFollowerConfig(MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
69+
() -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red,
70+
this);
71+
Pathfinding.setPathfinder(new LocalADStarAK());
72+
PathPlannerLogging.setLogActivePathCallback(
73+
(activePath) -> {
74+
Logger.recordOutput(
75+
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
76+
});
77+
PathPlannerLogging.setLogTargetPoseCallback(
78+
(targetPose) -> {
79+
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
80+
});
8981
}
9082

91-
// Stop moving when disabled
92-
if (DriverStation.isDisabled()) {
93-
for (var module : modules) {
94-
module.stop();
95-
}
83+
public void periodic() {
84+
gyroIO.updateInputs(gyroInputs);
85+
Logger.processInputs("Drive/Gyro", gyroInputs);
86+
for (var module : modules) {
87+
module.periodic();
88+
}
89+
90+
// Stop moving when disabled
91+
if (DriverStation.isDisabled()) {
92+
for (var module : modules) {
93+
module.stop();
94+
}
95+
}
96+
// Log empty setpoint states when disabled
97+
if (DriverStation.isDisabled()) {
98+
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
99+
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
100+
}
101+
102+
// Update odometry
103+
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
104+
for (int i = 0; i < 4; i++) {
105+
wheelDeltas[i] = modules[i].getPositionDelta();
106+
}
107+
// The twist represents the motion of the robot since the last
108+
// loop cycle in x, y, and theta based only on the modules,
109+
// without the gyro. The gyro is always disconnected in simulation.
110+
var twist = kinematics.toTwist2d(wheelDeltas);
111+
if (gyroInputs.connected) {
112+
// If the gyro is connected, replace the theta component of the twist
113+
// with the change in angle since the last loop cycle.
114+
twist =
115+
new Twist2d(
116+
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
117+
lastGyroRotation = gyroInputs.yawPosition;
118+
}
119+
// Apply the twist (change since last loop cycle) to the current pose
120+
pose = pose.exp(twist);
96121
}
97-
// Log empty setpoint states when disabled
98-
if (DriverStation.isDisabled()) {
99-
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
100-
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
122+
123+
/**
124+
* Runs the drive at the desired velocity.
125+
*
126+
* @param speeds Speeds in meters/sec
127+
*/
128+
public void runVelocity(ChassisSpeeds speeds) {
129+
// Calculate module setpoints
130+
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
131+
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
132+
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
133+
134+
// Send setpoints to modules
135+
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
136+
for (int i = 0; i < 4; i++) {
137+
// The module returns the optimized state, useful for logging
138+
optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i], forceModuleAngleChange);
139+
}
140+
// Log setpoint states
141+
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
142+
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
143+
}
144+
145+
/**
146+
* Stops the drive.
147+
*/
148+
public void stop() {
149+
runVelocity(new ChassisSpeeds());
150+
}
151+
152+
/**
153+
* Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
154+
* return to their normal orientations the next time a nonzero velocity is requested.
155+
*/
156+
public void stopWithX() {
157+
Rotation2d[] headings = new Rotation2d[4];
158+
for (int i = 0; i < 4; i++) {
159+
headings[i] = getModuleTranslations()[i].getAngle();
160+
}
161+
forceModuleAngleChange = true;
162+
kinematics.resetHeadings(headings);
163+
stop();
164+
forceModuleAngleChange = false;
165+
}
166+
167+
/**
168+
* Runs forwards at the commanded voltage.
169+
*/
170+
public void runCharacterizationVolts(double volts) {
171+
for (int i = 0; i < 4; i++) {
172+
modules[i].runCharacterization(volts);
173+
}
101174
}
102175

103-
// Update odometry
104-
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
105-
for (int i = 0; i < 4; i++) {
106-
wheelDeltas[i] = modules[i].getPositionDelta();
176+
/**
177+
* Returns the average drive velocity in radians/sec.
178+
*/
179+
public double getCharacterizationVelocity() {
180+
double driveVelocityAverage = 0.0;
181+
for (var module : modules) {
182+
driveVelocityAverage += module.getCharacterizationVelocity();
183+
}
184+
return driveVelocityAverage / 4.0;
107185
}
108-
// The twist represents the motion of the robot since the last
109-
// loop cycle in x, y, and theta based only on the modules,
110-
// without the gyro. The gyro is always disconnected in simulation.
111-
var twist = kinematics.toTwist2d(wheelDeltas);
112-
if (gyroInputs.connected) {
113-
// If the gyro is connected, replace the theta component of the twist
114-
// with the change in angle since the last loop cycle.
115-
twist =
116-
new Twist2d(
117-
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
118-
lastGyroRotation = gyroInputs.yawPosition;
186+
187+
/**
188+
* Returns the module states (turn angles and drive velocities) for all of the modules.
189+
*/
190+
@AutoLogOutput(key = "SwerveStates/Measured")
191+
private SwerveModuleState[] getModuleStates() {
192+
SwerveModuleState[] states = new SwerveModuleState[4];
193+
for (int i = 0; i < 4; i++) {
194+
states[i] = modules[i].getState();
195+
}
196+
return states;
119197
}
120-
// Apply the twist (change since last loop cycle) to the current pose
121-
pose = pose.exp(twist);
122-
}
123-
124-
/**
125-
* Runs the drive at the desired velocity.
126-
*
127-
* @param speeds Speeds in meters/sec
128-
*/
129-
public void runVelocity(ChassisSpeeds speeds) {
130-
// Calculate module setpoints
131-
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
132-
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
133-
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);
134-
135-
// Send setpoints to modules
136-
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
137-
for (int i = 0; i < 4; i++) {
138-
// The module returns the optimized state, useful for logging
139-
optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i], forceModuleAngleChange);
198+
199+
/**
200+
* Returns the current odometry pose.
201+
*/
202+
@AutoLogOutput(key = "Odometry/Robot")
203+
public Pose2d getPose() {
204+
return pose;
140205
}
141-
// Log setpoint states
142-
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
143-
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
144-
}
145-
146-
/** Stops the drive. */
147-
public void stop() {
148-
runVelocity(new ChassisSpeeds());
149-
}
150-
151-
/**
152-
* Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
153-
* return to their normal orientations the next time a nonzero velocity is requested.
154-
*/
155-
public void stopWithX() {
156-
Rotation2d[] headings = new Rotation2d[4];
157-
for (int i = 0; i < 4; i++) {
158-
headings[i] = getModuleTranslations()[i].getAngle();
206+
207+
/**
208+
* Returns the current odometry rotation.
209+
*/
210+
public Rotation2d getRotation() {
211+
return pose.getRotation();
159212
}
160-
forceModuleAngleChange = true;
161-
kinematics.resetHeadings(headings);
162-
stop();
163-
forceModuleAngleChange = false;
164-
}
165-
166-
/** Runs forwards at the commanded voltage. */
167-
public void runCharacterizationVolts(double volts) {
168-
for (int i = 0; i < 4; i++) {
169-
modules[i].runCharacterization(volts);
213+
214+
/**
215+
* Resets the current odometry pose.
216+
*/
217+
public void setPose(Pose2d pose) {
218+
this.pose = pose;
170219
}
171-
}
172220

173-
/** Returns the average drive velocity in radians/sec. */
174-
public double getCharacterizationVelocity() {
175-
double driveVelocityAverage = 0.0;
176-
for (var module : modules) {
177-
driveVelocityAverage += module.getCharacterizationVelocity();
221+
/**
222+
* Returns the maximum linear speed in meters per sec.
223+
*/
224+
public double getMaxLinearSpeedMetersPerSec() {
225+
return MAX_LINEAR_SPEED;
178226
}
179-
return driveVelocityAverage / 4.0;
180-
}
181-
182-
/** Returns the module states (turn angles and drive velocities) for all of the modules. */
183-
@AutoLogOutput(key = "SwerveStates/Measured")
184-
private SwerveModuleState[] getModuleStates() {
185-
SwerveModuleState[] states = new SwerveModuleState[4];
186-
for (int i = 0; i < 4; i++) {
187-
states[i] = modules[i].getState();
227+
228+
/**
229+
* Returns the maximum angular speed in radians per sec.
230+
*/
231+
public double getMaxAngularSpeedRadPerSec() {
232+
return MAX_ANGULAR_SPEED;
233+
}
234+
235+
/**
236+
* Returns an array of module translations.
237+
*/
238+
public static Translation2d[] getModuleTranslations() {
239+
return new Translation2d[]{
240+
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
241+
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
242+
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
243+
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
244+
};
188245
}
189-
return states;
190-
}
191-
192-
/** Returns the current odometry pose. */
193-
@AutoLogOutput(key = "Odometry/Robot")
194-
public Pose2d getPose() {
195-
return pose;
196-
}
197-
198-
/** Returns the current odometry rotation. */
199-
public Rotation2d getRotation() {
200-
return pose.getRotation();
201-
}
202-
203-
/** Resets the current odometry pose. */
204-
public void setPose(Pose2d pose) {
205-
this.pose = pose;
206-
}
207-
208-
/** Returns the maximum linear speed in meters per sec. */
209-
public double getMaxLinearSpeedMetersPerSec() {
210-
return MAX_LINEAR_SPEED;
211-
}
212-
213-
/** Returns the maximum angular speed in radians per sec. */
214-
public double getMaxAngularSpeedRadPerSec() {
215-
return MAX_ANGULAR_SPEED;
216-
}
217-
218-
/** Returns an array of module translations. */
219-
public static Translation2d[] getModuleTranslations() {
220-
return new Translation2d[] {
221-
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
222-
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
223-
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
224-
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
225-
};
226-
}
227246
}

0 commit comments

Comments
 (0)