Skip to content

Commit

Permalink
fix: passes checks (hopefully)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mimizh2418
mimizh2418 committed Jan 9, 2024
1 parent 630b56c commit c88d0d9
Showing 1 changed file with 192 additions and 173 deletions.
365 changes: 192 additions & 173 deletions src/main/java/frc/robot/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.util.LocalADStarAK;
Expand All @@ -39,189 +38,209 @@
public class Drive extends SubsystemBase {


private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final Module[] modules = new Module[4]; // FL, FR, BL, BR

private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Pose2d pose = new Pose2d();
private Rotation2d lastGyroRotation = new Rotation2d();
private boolean forceModuleAngleChange = false;

public Drive(
GyroIO gyroIO,
ModuleIO flModuleIO,
ModuleIO frModuleIO,
ModuleIO blModuleIO,
ModuleIO brModuleIO) {
this.gyroIO = gyroIO;
modules[0] = new Module(flModuleIO, 0);
modules[1] = new Module(frModuleIO, 1);
modules[2] = new Module(blModuleIO, 2);
modules[3] = new Module(brModuleIO, 3);

// Configure AutoBuilder for PathPlanner
AutoBuilder.configureHolonomic(
this::getPose,
this::setPose,
() -> kinematics.toChassisSpeeds(getModuleStates()),
this::runVelocity,
new HolonomicPathFollowerConfig(
MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
this);
Pathfinding.setPathfinder(new LocalADStarAK());
PathPlannerLogging.setLogActivePathCallback(
(activePath) -> {
Logger.recordOutput(
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
});
PathPlannerLogging.setLogTargetPoseCallback(
(targetPose) -> {
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
});
}

public void periodic() {
gyroIO.updateInputs(gyroInputs);
Logger.processInputs("Drive/Gyro", gyroInputs);
for (var module : modules) {
module.periodic();
private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final Module[] modules = new Module[4]; // FL, FR, BL, BR

private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
private Pose2d pose = new Pose2d();
private Rotation2d lastGyroRotation = new Rotation2d();
private boolean forceModuleAngleChange = false;

public Drive(
GyroIO gyroIO,
ModuleIO flModuleIO,
ModuleIO frModuleIO,
ModuleIO blModuleIO,
ModuleIO brModuleIO) {
this.gyroIO = gyroIO;
modules[0] = new Module(flModuleIO, 0);
modules[1] = new Module(frModuleIO, 1);
modules[2] = new Module(blModuleIO, 2);
modules[3] = new Module(brModuleIO, 3);

// Configure AutoBuilder for PathPlanner
AutoBuilder.configureHolonomic(
this::getPose,
this::setPose,
() -> kinematics.toChassisSpeeds(getModuleStates()),
this::runVelocity,
new HolonomicPathFollowerConfig(MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
() -> DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red,
this);
Pathfinding.setPathfinder(new LocalADStarAK());
PathPlannerLogging.setLogActivePathCallback(
(activePath) -> {
Logger.recordOutput(
"Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
});
PathPlannerLogging.setLogTargetPoseCallback(
(targetPose) -> {
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
});
}

// Stop moving when disabled
if (DriverStation.isDisabled()) {
for (var module : modules) {
module.stop();
}
public void periodic() {
gyroIO.updateInputs(gyroInputs);
Logger.processInputs("Drive/Gyro", gyroInputs);
for (var module : modules) {
module.periodic();
}

// Stop moving when disabled
if (DriverStation.isDisabled()) {
for (var module : modules) {
module.stop();
}
}
// Log empty setpoint states when disabled
if (DriverStation.isDisabled()) {
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
}

// Update odometry
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
for (int i = 0; i < 4; i++) {
wheelDeltas[i] = modules[i].getPositionDelta();
}
// The twist represents the motion of the robot since the last
// loop cycle in x, y, and theta based only on the modules,
// without the gyro. The gyro is always disconnected in simulation.
var twist = kinematics.toTwist2d(wheelDeltas);
if (gyroInputs.connected) {
// If the gyro is connected, replace the theta component of the twist
// with the change in angle since the last loop cycle.
twist =
new Twist2d(
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
lastGyroRotation = gyroInputs.yawPosition;
}
// Apply the twist (change since last loop cycle) to the current pose
pose = pose.exp(twist);
}
// Log empty setpoint states when disabled
if (DriverStation.isDisabled()) {
Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});

/**
* Runs the drive at the desired velocity.
*
* @param speeds Speeds in meters/sec
*/
public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);

// Send setpoints to modules
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
for (int i = 0; i < 4; i++) {
// The module returns the optimized state, useful for logging
optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i], forceModuleAngleChange);
}
// Log setpoint states
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
}

/**
* Stops the drive.
*/
public void stop() {
runVelocity(new ChassisSpeeds());
}

/**
* Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
* return to their normal orientations the next time a nonzero velocity is requested.
*/
public void stopWithX() {
Rotation2d[] headings = new Rotation2d[4];
for (int i = 0; i < 4; i++) {
headings[i] = getModuleTranslations()[i].getAngle();
}
forceModuleAngleChange = true;
kinematics.resetHeadings(headings);
stop();
forceModuleAngleChange = false;
}

/**
* Runs forwards at the commanded voltage.
*/
public void runCharacterizationVolts(double volts) {
for (int i = 0; i < 4; i++) {
modules[i].runCharacterization(volts);
}
}

// Update odometry
SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
for (int i = 0; i < 4; i++) {
wheelDeltas[i] = modules[i].getPositionDelta();
/**
* Returns the average drive velocity in radians/sec.
*/
public double getCharacterizationVelocity() {
double driveVelocityAverage = 0.0;
for (var module : modules) {
driveVelocityAverage += module.getCharacterizationVelocity();
}
return driveVelocityAverage / 4.0;
}
// The twist represents the motion of the robot since the last
// loop cycle in x, y, and theta based only on the modules,
// without the gyro. The gyro is always disconnected in simulation.
var twist = kinematics.toTwist2d(wheelDeltas);
if (gyroInputs.connected) {
// If the gyro is connected, replace the theta component of the twist
// with the change in angle since the last loop cycle.
twist =
new Twist2d(
twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
lastGyroRotation = gyroInputs.yawPosition;

/**
* Returns the module states (turn angles and drive velocities) for all of the modules.
*/
@AutoLogOutput(key = "SwerveStates/Measured")
private SwerveModuleState[] getModuleStates() {
SwerveModuleState[] states = new SwerveModuleState[4];
for (int i = 0; i < 4; i++) {
states[i] = modules[i].getState();
}
return states;
}
// Apply the twist (change since last loop cycle) to the current pose
pose = pose.exp(twist);
}

/**
* Runs the drive at the desired velocity.
*
* @param speeds Speeds in meters/sec
*/
public void runVelocity(ChassisSpeeds speeds) {
// Calculate module setpoints
ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED);

// Send setpoints to modules
SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4];
for (int i = 0; i < 4; i++) {
// The module returns the optimized state, useful for logging
optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i], forceModuleAngleChange);

/**
* Returns the current odometry pose.
*/
@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getPose() {
return pose;
}
// Log setpoint states
Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
}

/** Stops the drive. */
public void stop() {
runVelocity(new ChassisSpeeds());
}

/**
* Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
* return to their normal orientations the next time a nonzero velocity is requested.
*/
public void stopWithX() {
Rotation2d[] headings = new Rotation2d[4];
for (int i = 0; i < 4; i++) {
headings[i] = getModuleTranslations()[i].getAngle();

/**
* Returns the current odometry rotation.
*/
public Rotation2d getRotation() {
return pose.getRotation();
}
forceModuleAngleChange = true;
kinematics.resetHeadings(headings);
stop();
forceModuleAngleChange = false;
}

/** Runs forwards at the commanded voltage. */
public void runCharacterizationVolts(double volts) {
for (int i = 0; i < 4; i++) {
modules[i].runCharacterization(volts);

/**
* Resets the current odometry pose.
*/
public void setPose(Pose2d pose) {
this.pose = pose;
}
}

/** Returns the average drive velocity in radians/sec. */
public double getCharacterizationVelocity() {
double driveVelocityAverage = 0.0;
for (var module : modules) {
driveVelocityAverage += module.getCharacterizationVelocity();
/**
* Returns the maximum linear speed in meters per sec.
*/
public double getMaxLinearSpeedMetersPerSec() {
return MAX_LINEAR_SPEED;
}
return driveVelocityAverage / 4.0;
}

/** Returns the module states (turn angles and drive velocities) for all of the modules. */
@AutoLogOutput(key = "SwerveStates/Measured")
private SwerveModuleState[] getModuleStates() {
SwerveModuleState[] states = new SwerveModuleState[4];
for (int i = 0; i < 4; i++) {
states[i] = modules[i].getState();

/**
* Returns the maximum angular speed in radians per sec.
*/
public double getMaxAngularSpeedRadPerSec() {
return MAX_ANGULAR_SPEED;
}

/**
* Returns an array of module translations.
*/
public static Translation2d[] getModuleTranslations() {
return new Translation2d[]{
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
};
}
return states;
}

/** Returns the current odometry pose. */
@AutoLogOutput(key = "Odometry/Robot")
public Pose2d getPose() {
return pose;
}

/** Returns the current odometry rotation. */
public Rotation2d getRotation() {
return pose.getRotation();
}

/** Resets the current odometry pose. */
public void setPose(Pose2d pose) {
this.pose = pose;
}

/** Returns the maximum linear speed in meters per sec. */
public double getMaxLinearSpeedMetersPerSec() {
return MAX_LINEAR_SPEED;
}

/** Returns the maximum angular speed in radians per sec. */
public double getMaxAngularSpeedRadPerSec() {
return MAX_ANGULAR_SPEED;
}

/** Returns an array of module translations. */
public static Translation2d[] getModuleTranslations() {
return new Translation2d[] {
new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0),
new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0)
};
}
}

0 comments on commit c88d0d9

Please sign in to comment.