From dec1be6e7173996238818aaf5dd69a3c549b59c0 Mon Sep 17 00:00:00 2001 From: iefomit <108955303+iefomit@users.noreply.github.com> Date: Sun, 23 Aug 2026 21:52:00 -0700 Subject: [PATCH] I love api changes --- .../drive_comm/DefaultDriveCommand.java | 16 ++++---- .../commands/drive_comm/DriveToPose.java | 4 +- .../commands/drive_comm/SetFormationX.java | 13 ++++--- .../drive_comm/SimplePresetSteerAngles.java | 12 +++--- .../TrajectoryPresetSteerAngles.java | 16 ++++---- .../robot/commands/vision/AimAtGamePiece.java | 4 +- .../frc/robot/commands/vision/GoToPose2.java | 4 +- .../subsystems/drivetrain/Drivetrain.java | 38 +++++++++---------- .../robot/subsystems/drivetrain/Module.java | 14 +++---- .../subsystems/drivetrain/ModuleSim.java | 12 +++--- src/main/java/frc/robot/util/GeomUtil.java | 2 +- .../java/frc/robot/util/SwerveModulePose.java | 6 +-- .../util/SwerveStuff/SwerveSetpoint.java | 4 +- .../SwerveStuff/SwerveSetpointGenerator.java | 22 +++++------ .../frc/robot/util/Vision/DetectedObject.java | 2 +- .../frc/robot/util/Vision/DriverAssist.java | 16 ++++---- src/main/java/lib/CTREModuleState.java | 8 ++-- 17 files changed, 97 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index a25c8a7..f955075 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -53,27 +53,27 @@ public class DefaultDriveCommand extends Command { forwardTranslation *= allianceReversal; sideTranslation *= allianceReversal; - ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); - ChassisSpeeds corrected = + ChassisVelocities driverInput = new ChassisVelocities(forwardTranslation, sideTranslation, rotation); + ChassisVelocities corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true); } /** * Drives the robot * - * @param speeds The ChassisSpeeds to drive at + * @param speeds The ChassisVelocities to drive at */ - protected void drive(ChassisSpeeds speeds) { + protected void drive(ChassisVelocities speeds) { // If the driver is pressing the align button or a command set the drivetrain to // align, then align to speaker if (driver.getIsAlign() || swerve.getIsAlign()) { swerve.driveHeading( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, swerve.getAlignAngle(), true); + speeds.vx, speeds.vy, swerve.getAlignAngle(), true); } else { swerve.drive( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, + speeds.vx, + speeds.vy, + speeds.omega, true, false); } diff --git a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java index 7c6d014..f5c2a0d 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java +++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java @@ -105,10 +105,10 @@ public class DriveToPose extends Command { ChassisVelocities fieldVelocity = ChassisVelocities.fromRobotRelativeSpeeds(drive.getChassisVelocities(), currentPose.getRotation()); Translation2d linearFieldVelocity = - new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond); + new Translation2d(fieldVelocity.vx, fieldVelocity.vy); thetaController.reset( - currentPose.getRotation().getRadians(), fieldVelocity.omegaRadiansPerSecond); + currentPose.getRotation().getRadians(), fieldVelocity.omega); lastSetpointTranslation = currentPose.getTranslation(); if (targetPose != null) { diff --git a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java index 37bd539..7d6ba31 100644 --- a/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java +++ b/src/main/java/frc/robot/commands/drive_comm/SetFormationX.java @@ -1,7 +1,8 @@ package frc.robot.commands.drive_comm; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; import org.wpilib.command2.InstantCommand; import org.wpilib.command2.RunCommand; @@ -17,11 +18,11 @@ public class SetFormationX extends SequentialCommandGroup { new RunCommand( () -> drive.setModuleStates( - new SwerveModuleState[] { - new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))), - new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))), - new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(-45))), - new SwerveModuleState(0, new Rotation2d(Units.degreesToRadians(45))) + new SwerveModuleVelocity[] { + new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(45))), + new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(-45))), + new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(-45))), + new SwerveModuleVelocity(0, new Rotation2d(Units.degreesToRadians(45))) }, false), drive)); diff --git a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java index 4fba8b1..a9602e5 100644 --- a/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java +++ b/src/main/java/frc/robot/commands/drive_comm/SimplePresetSteerAngles.java @@ -1,7 +1,7 @@ package frc.robot.commands.drive_comm; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.command2.InstantCommand; import frc.robot.subsystems.drivetrain.Drivetrain; @@ -41,11 +41,11 @@ public class SimplePresetSteerAngles extends InstantCommand { () -> { drive.setStateDeadband(false); drive.setModuleStates( - new SwerveModuleState[] { - new SwerveModuleState(0, rotation), - new SwerveModuleState(0, rotation), - new SwerveModuleState(0, rotation), - new SwerveModuleState(0, rotation) + new SwerveModuleVelocity[] { + new SwerveModuleVelocity(0, rotation), + new SwerveModuleVelocity(0, rotation), + new SwerveModuleVelocity(0, rotation), + new SwerveModuleVelocity(0, rotation) }, true); drive.setStateDeadband(true); diff --git a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java index 62abbe6..3ba8916 100644 --- a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java +++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java @@ -2,7 +2,7 @@ package frc.robot.commands.drive_comm; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.kinematics.ChassisVelocities; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.trajectory.Trajectory; import org.wpilib.math.trajectory.Trajectory.State; import org.wpilib.command2.InstantCommand; @@ -29,8 +29,8 @@ public class TrajectoryPresetSteerAngles extends InstantCommand { State sample = trajectory.sample(time); Pose2d nextPose = sample.poseMeters; - double xVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getCos(); - double yVelocity = sample.velocityMetersPerSecond * nextPose.getRotation().getSin(); + double xVelocity = sample.velocity * nextPose.getRotation().getCos(); + double yVelocity = sample.velocity * nextPose.getRotation().getSin(); double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time; @@ -38,12 +38,12 @@ public class TrajectoryPresetSteerAngles extends InstantCommand { chassisSpeeds = ChassisVelocities.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation()); - SwerveModuleState[] swerveModuleStates = - DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds); - for (SwerveModuleState swerveModuleState : swerveModuleStates) { - swerveModuleState.speedMetersPerSecond = 0; + SwerveModuleVelocity[] SwerveModuleVelocitys = + DriveConstants.KINEMATICS.toSwerveModuleVelocitys(chassisSpeeds); + for (SwerveModuleVelocity SwerveModuleVelocity : SwerveModuleVelocitys) { + SwerveModuleVelocity.speedMetersPerSecond = 0; } - drive.setModuleStates(swerveModuleStates, true); + drive.setModuleStates(SwerveModuleVelocitys, true); drive.setStateDeadband(true); }, drive); diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java index b83303a..506c619 100644 --- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -51,8 +51,8 @@ public class AimAtGamePiece extends DefaultDriveCommand { // System.out.println("objangle " + object.getAngle()); swerve.driveHeading( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, + speeds.vx, + speeds.vy, MathUtil.angleModulus(object.getAngle()), true); } diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java index 974ea57..c42be32 100644 --- a/src/main/java/frc/robot/commands/vision/GoToPose2.java +++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java @@ -28,8 +28,8 @@ public class GoToPose2 extends Command { public void initialize() { pose = poseSupplier.get(); ChassisVelocities v = drive.getChassisVelocities(); - vx = v.vxMetersPerSecond; - vy = v.vyMetersPerSecond; + vx = v.vx; + vy = v.vy; error = null; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 809c554..1e2d12c 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -22,7 +22,7 @@ import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Voltage; import org.wpilib.framework.RobotBase; @@ -63,11 +63,11 @@ public class Drivetrain extends SubsystemBase { private SwerveSetpoint currentSetpoint = new SwerveSetpoint( new ChassisVelocities(), - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() + new SwerveModuleVelocity[] { + new SwerveModuleVelocity(), + new SwerveModuleVelocity(), + new SwerveModuleVelocity(), + new SwerveModuleVelocity() }); // Odometry private final SwerveDrivePoseEstimator poseEstimator; @@ -421,7 +421,7 @@ public class Drivetrain extends SubsystemBase { // if (Robot.isSimulation()) { // pigeon.getSimState().addYaw( - // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omegaRadiansPerSecond + // +Units.radiansToDegrees(currentSetpoint.chassisSpeeds().omega // * Constants.LOOP_TIME)); // } } @@ -462,15 +462,15 @@ public class Drivetrain extends SubsystemBase { /** * Sets the desired states for all swerve modules. * - * @param swerveModuleStates an array of module states to set swerve modules to. Order of the + * @param SwerveModuleVelocitys an array of module states to set swerve modules to. Order of the * array matters here! */ - public void setModuleStates(SwerveModuleState[] swerveModuleStates, boolean isOpenLoop) { + public void setModuleStates(SwerveModuleVelocity[] SwerveModuleVelocitys, boolean isOpenLoop) { // makes sure speeds of modules don't exceed maximum allowed - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.MAX_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(SwerveModuleVelocitys, DriveConstants.MAX_SPEED); for (int i = 0; i < 4; i++) { - modules[i].setDesiredState(swerveModuleStates[i], isOpenLoop); + modules[i].setDesiredState(SwerveModuleVelocitys[i], isOpenLoop); } } @@ -501,8 +501,8 @@ public class Drivetrain extends SubsystemBase { Constants.LOOP_TIME); } - SwerveModuleState[] swerveModuleStates = currentSetpoint.moduleStates(); - setModuleStates(swerveModuleStates, isOpenLoop); + SwerveModuleVelocity[] SwerveModuleVelocitys = currentSetpoint.moduleStates(); + setModuleStates(SwerveModuleVelocitys, isOpenLoop); } public void setDriveVoltages(Voltage voltage) { @@ -587,7 +587,7 @@ public class Drivetrain extends SubsystemBase { } /** - * Calculates chassis speed of drivetrain using the current SwerveModuleStates + * Calculates chassis speed of drivetrain using the current SwerveModuleVelocitys * * @return ChassisVelocities object This is often used as an input for other methods */ @@ -598,10 +598,10 @@ public class Drivetrain extends SubsystemBase { /** * Gets the state of each module * - * @return An array of 4 SwerveModuleStates + * @return An array of 4 SwerveModuleVelocitys */ - public SwerveModuleState[] getModuleStates() { - return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + public SwerveModuleVelocity[] getModuleStates() { + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleVelocity[]::new); } public SwerveSetpoint getCurrSetpoint() { @@ -843,7 +843,7 @@ public class Drivetrain extends SubsystemBase { } public void alignWheels() { - SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0)); - setModuleStates(new SwerveModuleState[] {state, state, state, state}, false); + SwerveModuleVelocity state = new SwerveModuleVelocity(0, new Rotation2d(0)); + setModuleStates(new SwerveModuleVelocity[] {state, state, state, state}, false); } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java index 9caf1d6..1affc0d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -26,7 +26,7 @@ import org.wpilib.math.util.MathUtil; import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.math.util.Units; import org.wpilib.units.measure.Angle; import frc.robot.constants.Constants; @@ -50,7 +50,7 @@ public class Module implements ModuleIO { private final TalonFX angleMotor; private final TalonFX driveMotor; private final CANcoder CANcoder; - private SwerveModuleState desiredState; + private SwerveModuleVelocity desiredState; protected boolean stateDeadband = true; @@ -158,7 +158,7 @@ public class Module implements ModuleIO { turnAppliedVolts, turnCurrent); - setDesiredState(new SwerveModuleState(0, getAngle()), false); + setDesiredState(new SwerveModuleVelocity(0, getAngle()), false); } public void close() { @@ -252,7 +252,7 @@ public class Module implements ModuleIO { } } - public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { + public void setDesiredState(SwerveModuleVelocity wantedState, boolean isOpenLoop) { // Separate if here and in setAngle() to avoid warning if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { /* @@ -481,8 +481,8 @@ public class Module implements ModuleIO { driveMotor.optimizeBusUtilization(); } - public SwerveModuleState getState() { - return new SwerveModuleState( + public SwerveModuleVelocity getState() { + return new SwerveModuleVelocity( inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, getAngle()); } @@ -491,7 +491,7 @@ public class Module implements ModuleIO { inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, getAngle()); } - public SwerveModuleState getDesiredState() { + public SwerveModuleVelocity getDesiredState() { return desiredState; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java index f6a3d6b..c9f395c 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/ModuleSim.java @@ -4,7 +4,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.kinematics.SwerveModulePosition; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import org.wpilib.system.Timer; import frc.robot.constants.Constants; import frc.robot.constants.swerve.DriveConstants; @@ -21,7 +21,7 @@ public class ModuleSim extends Module { private double currentDrivePositionMeters = 0; private double currentSpeed = 0; - private SwerveModuleState desiredState; + private SwerveModuleVelocity desiredState; protected boolean stateDeadband = true; @@ -71,7 +71,7 @@ public class ModuleSim extends Module { * @param desiredState Desired state with speed and angle. * @param isOpenLoop whether to use closed/open loop control for drive velocity */ - public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + public void setDesiredState(SwerveModuleVelocity desiredState, boolean isOpenLoop) { if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { // If the module isn't moving, don't rotate it if (Math.abs(desiredState.speedMetersPerSecond) < 0.001) { @@ -91,7 +91,7 @@ public class ModuleSim extends Module { // does nothing when robot does not have a swerve drivetrain } - public SwerveModuleState getDesiredState() { + public SwerveModuleVelocity getDesiredState() { return desiredState; } @@ -108,8 +108,8 @@ public class ModuleSim extends Module { currentSpeed = 0; } - public SwerveModuleState getState() { - return new SwerveModuleState(currentSpeed, getAngle()); + public SwerveModuleVelocity getState() { + return new SwerveModuleVelocity(currentSpeed, getAngle()); } public SwerveModulePosition getPosition() { diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 67e67b5..c44a40b 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -136,7 +136,7 @@ public class GeomUtil { */ public static Twist2d toTwist2d(ChassisVelocities speeds) { return new Twist2d( - speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + speeds.vx, speeds.vy, speeds.omega); } /** diff --git a/src/main/java/frc/robot/util/SwerveModulePose.java b/src/main/java/frc/robot/util/SwerveModulePose.java index 4b90f0b..ef4fc43 100644 --- a/src/main/java/frc/robot/util/SwerveModulePose.java +++ b/src/main/java/frc/robot/util/SwerveModulePose.java @@ -11,7 +11,7 @@ import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import frc.robot.subsystems.drivetrain.Drivetrain; /** Stores and updates the position of each module */ @@ -43,7 +43,7 @@ public class SwerveModulePose { /** Updates the module positions */ public void update() { - SwerveModuleState[] states = drive.getModuleStates(); + SwerveModuleVelocity[] states = drive.getModuleStates(); double currentRotation = drive.getYaw().getRadians(); double chassisRotation = currentRotation - prevRotation; @@ -84,7 +84,7 @@ public class SwerveModulePose { /** Resets the modules to the correct positions relative to the robot */ public void reset() { Pose2d chassisPose2d = drive.getPose(); - SwerveModuleState[] states = drive.getModuleStates(); + SwerveModuleVelocity[] states = drive.getModuleStates(); for (int i = 0; i < 4; i++) { angles[i] = states[i].angle.getRadians(); this.modulePositions[i] = diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java index 3dda108..609ff6e 100644 --- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java @@ -8,6 +8,6 @@ package frc.robot.util.SwerveStuff; import org.wpilib.math.kinematics.ChassisVelocities; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; -public record SwerveSetpoint(ChassisVelocities chassisSpeeds, SwerveModuleState[] moduleStates) {} +public record SwerveSetpoint(ChassisVelocities chassisSpeeds, SwerveModuleVelocity[] moduleStates) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java index 8039f56..0c8f7bc 100644 --- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java @@ -14,7 +14,7 @@ import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Twist2d; import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveDriveKinematics; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -266,7 +266,7 @@ public class SwerveSetpointGenerator { double dt) { final Translation2d[] modules = moduleLocations; - SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + SwerveModuleVelocity[] desiredModuleState = kinematics.toSwerveModuleVelocitys(desiredState); // Make sure desiredState respects velocity limits. if (limits.maxDriveVelocity() > 0.0) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); @@ -334,10 +334,10 @@ public class SwerveSetpointGenerator { // the goal state; then // find the amount we can move from start towards goal in this cycle such that no kinematic // limit is exceeded. - double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; - double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dx = desiredState.vx - prevSetpoint.chassisSpeeds().vx; + double dy = desiredState.vy - prevSetpoint.chassisSpeeds().vy; double dtheta = - desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + desiredState.omega - prevSetpoint.chassisSpeeds().omega; // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at // desiredState. @@ -455,10 +455,10 @@ public class SwerveSetpointGenerator { // x and y are limited separately because, when tipping in a diagonal direction, the distance // is longer double xAccel = - Math.abs(desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond) + Math.abs(desiredState.vx - prevSetpoint.chassisSpeeds().vx) / dt; double yAccel = - Math.abs(desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond) + Math.abs(desiredState.vy - prevSetpoint.chassisSpeeds().vy) / dt; if (!epsilonEquals(xAccel, 0)) { double s = maxAccel / xAccel; @@ -472,10 +472,10 @@ public class SwerveSetpointGenerator { ChassisVelocities retSpeeds = new ChassisVelocities( - prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, - prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); - var retStates = kinematics.toSwerveModuleStates(retSpeeds); + prevSetpoint.chassisSpeeds().vx + min_s * dx, + prevSetpoint.chassisSpeeds().vy + min_s * dy, + prevSetpoint.chassisSpeeds().omega + min_s * dtheta); + var retStates = kinematics.toSwerveModuleVelocitys(retSpeeds); for (int i = 0; i < modules.length; ++i) { final var maybeOverride = overrideSteering.get(i); if (maybeOverride.isPresent()) { diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java index cc0f81d..b9f81d4 100644 --- a/src/main/java/frc/robot/util/Vision/DetectedObject.java +++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java @@ -372,7 +372,7 @@ public class DetectedObject { public double getVelocityRelativeAngle() { ChassisVelocities speeds = drive.getChassisVelocities(); double angle = - getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond); + getRelativeAngle() - Math.atan2(speeds.vy, speeds.vx); return MathUtil.angleModulus(angle); } diff --git a/src/main/java/frc/robot/util/Vision/DriverAssist.java b/src/main/java/frc/robot/util/Vision/DriverAssist.java index c50c9ff..f3a1f0c 100644 --- a/src/main/java/frc/robot/util/Vision/DriverAssist.java +++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java @@ -67,10 +67,10 @@ public class DriverAssist { driveSpeeds, yaw); // Changing this does not cause problems because getChassisVelocities() creates a new // object - State xState = new State(currentPose.getX(), driveSpeeds.vxMetersPerSecond); - State yState = new State(currentPose.getY(), driveSpeeds.vyMetersPerSecond); + State xState = new State(currentPose.getX(), driveSpeeds.vx); + State yState = new State(currentPose.getY(), driveSpeeds.vy); State angleState = - new State(currentPose.getRotation().getRadians(), driveSpeeds.omegaRadiansPerSecond); + new State(currentPose.getRotation().getRadians(), driveSpeeds.omega); // Store goal states State xGoal = new State(desiredPose.getX(), 0); @@ -133,7 +133,7 @@ public class DriverAssist { // Driver input speed double driverInputSpeed = - Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond); + Math.hypot(driverInput.vx, driverInput.vy); // The amount to correct by ChassisVelocities correction = @@ -144,7 +144,7 @@ public class DriverAssist { return driverSpeeds.plus(correction); // return - // nextChassisSpeed.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR)); + // nextChassisVelocities.times(CORRECTION_FACTOR).plus(driverInput.times(1-CORRECTION_FACTOR)); } // Constants used for second method @@ -180,8 +180,8 @@ public class DriverAssist { keepAngle ? desiredPose.getRotation().getRadians() : MathUtil.angleModulus(velocityAngle + Math.PI / 2); - double inputSpeed = Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond); - double driverAngle = Math.atan2(driverInput.vyMetersPerSecond, driverInput.vxMetersPerSecond); + double inputSpeed = Math.hypot(driverInput.vx, driverInput.vy); + double driverAngle = Math.atan2(driverInput.vy, driverInput.vx); double velocityAngleError = MathUtil.angleModulus(velocityAngle - driverAngle); if (Math.abs(velocityAngleError) > MAX_VELOCITY_ANGLE_ERROR) { return driverInput; @@ -229,7 +229,7 @@ public class DriverAssist { ROTATION_CORRECTION_FACTOR * Math.signum(rotationError) * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError)) - - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond; + - ROTATION_CORRECTION_FACTOR * driverInput.omega; return driverInput.plus( new ChassisVelocities( correctionSpeed * Math.cos(perpendicularAngle), diff --git a/src/main/java/lib/CTREModuleState.java b/src/main/java/lib/CTREModuleState.java index 8a90f5b..fb8742b 100644 --- a/src/main/java/lib/CTREModuleState.java +++ b/src/main/java/lib/CTREModuleState.java @@ -1,7 +1,7 @@ package lib; import org.wpilib.math.geometry.Rotation2d; -import org.wpilib.math.kinematics.SwerveModuleState; +import org.wpilib.math.kinematics.SwerveModuleVelocity; public class CTREModuleState { @@ -13,8 +13,8 @@ public class CTREModuleState { * @param desiredState The desired state. * @param currentAngle The current module angle. */ - public static SwerveModuleState optimize( - SwerveModuleState desiredState, Rotation2d currentAngle) { + public static SwerveModuleVelocity optimize( + SwerveModuleVelocity desiredState, Rotation2d currentAngle) { double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees()); double targetSpeed = desiredState.speedMetersPerSecond; @@ -27,7 +27,7 @@ public class CTREModuleState { targetAngle += 180; } } - return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle)); + return new SwerveModuleVelocity(targetSpeed, Rotation2d.fromDegrees(targetAngle)); } /** -- 2.47.3