From d08bdfa1136c0da488fda97d618853b822f37a44 Mon Sep 17 00:00:00 2001 From: iefomit <108955303+iefomit@users.noreply.github.com> Date: Sun, 23 Aug 2026 10:18:45 -0700 Subject: [PATCH] first few error fixes --- src/main/java/frc/robot/RobotContainer.java | 8 ++-- .../auto_comm/ChoreoPathCommandBuilder.java | 33 ++++++++------- .../drive_comm/DefaultDriveCommand.java | 2 +- .../commands/drive_comm/DriveToPose.java | 8 ++-- .../TrajectoryPresetSteerAngles.java | 6 +-- .../robot/commands/vision/AimAtGamePiece.java | 4 +- .../frc/robot/commands/vision/GoToPose2.java | 4 +- .../subsystems/drivetrain/Drivetrain.java | 26 ++++++------ src/main/java/frc/robot/util/GeomUtil.java | 6 +-- .../util/SwerveStuff/SwerveSetpoint.java | 4 +- .../SwerveStuff/SwerveSetpointGenerator.java | 14 +++---- .../frc/robot/util/Vision/DetectedObject.java | 4 +- .../frc/robot/util/Vision/DriverAssist.java | 42 +++++++++---------- 13 files changed, 81 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26d35a7..5d21939 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,7 +149,7 @@ public class RobotContainer { new AutoFactory( drive::getPose, drive::resetOdometry, - sample -> drive.setChassisSpeeds(sample.getChassisSpeeds(), false), + sample -> drive.setChassisVelocities(sample.getChassisVelocities(), false), true, drive, (trajectory, startOrFinish) -> { @@ -172,12 +172,12 @@ public class RobotContainer { (pose) -> { drive.resetOdometry(pose); }, - () -> drive.getChassisSpeeds(), + () -> drive.getChassisVelocities(), (chassisSpeeds) -> { if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Auto/ChassisSpeeds", chassisSpeeds); + Logger.recordOutput("Auto/ChassisVelocities", chassisSpeeds); } - drive.setChassisSpeeds(chassisSpeeds, false); // problem?? + drive.setChassisVelocities(chassisSpeeds, false); // problem?? }, AutoConstants.AUTO_CONTROLLER, AutoConstants.CONFIG, diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java index 888e1a1..393aec6 100644 --- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java @@ -1,21 +1,22 @@ -package frc.robot.commands.auto_comm; +// TODO: 2027-ALPHA-FIX - Re-enable Choreo vendordep when JSON is updated +// package frc.robot.commands.auto_comm; -import choreo.auto.AutoFactory; -import org.wpilib.command2.Command; -import org.wpilib.command2.Commands; -import org.wpilib.command2.InstantCommand; -import frc.robot.commands.DoNothing; +// import choreo.auto.AutoFactory; +// import org.wpilib.command2.Command; +// import org.wpilib.command2.Commands; +// import org.wpilib.command2.InstantCommand; +// import frc.robot.commands.DoNothing; -public class ChoreoPathCommandBuilder { +// public class ChoreoPathCommandBuilder { - public ChoreoPathCommandBuilder() {} +// public ChoreoPathCommandBuilder() {} - public static Command basicTrajectoryAuto( - String pathName, boolean resetOdemetry, AutoFactory factory) { - Command command = factory.trajectoryCmd(pathName); +// public static Command basicTrajectoryAuto( +// String pathName, boolean resetOdemetry, AutoFactory factory) { +// Command command = factory.trajectoryCmd(pathName); - return Commands.sequence( - resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(), - command); - } -} +// return Commands.sequence( +// resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(), +// command); +// } +// } 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 5ddaa9a..a25c8a7 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands.drive_comm; import org.wpilib.math.controller.PIDController; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.driverstation.DriverStation.Alliance; import org.wpilib.smartdashboard.SmartDashboard; import org.wpilib.command2.Command; 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 d2331db..7c6d014 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java +++ b/src/main/java/frc/robot/commands/drive_comm/DriveToPose.java @@ -16,7 +16,7 @@ import org.wpilib.math.filter.Debouncer; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Units; import org.wpilib.command2.Command; @@ -102,8 +102,8 @@ public class DriveToPose extends Command { targetPose = target.get(); Pose2d currentPose = robot.get(); - ChassisSpeeds fieldVelocity = - ChassisSpeeds.fromRobotRelativeSpeeds(drive.getChassisSpeeds(), currentPose.getRotation()); + ChassisVelocities fieldVelocity = + ChassisVelocities.fromRobotRelativeSpeeds(drive.getChassisVelocities(), currentPose.getRotation()); Translation2d linearFieldVelocity = new Translation2d(fieldVelocity.vxMetersPerSecond, fieldVelocity.vyMetersPerSecond); @@ -141,7 +141,7 @@ public class DriveToPose extends Command { // Calculate drive speed double currentDistance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); double ffScaler = - MathUtil.clamp((currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius), 0.0, 1.0); + Math.max(0.0, Math.min(1.0,(currentDistance - ffMinRadius) / (ffMaxRadius - ffMinRadius))); driveErrorAbs = currentDistance; driveController.reset( lastSetpointTranslation.getDistance(targetPose.getTranslation()), 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 87b77a4..62abbe6 100644 --- a/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java +++ b/src/main/java/frc/robot/commands/drive_comm/TrajectoryPresetSteerAngles.java @@ -1,7 +1,7 @@ package frc.robot.commands.drive_comm; import org.wpilib.math.geometry.Pose2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveModuleState; import org.wpilib.math.trajectory.Trajectory; import org.wpilib.math.trajectory.Trajectory.State; @@ -34,9 +34,9 @@ public class TrajectoryPresetSteerAngles extends InstantCommand { double angularVelo = (nextPose.getRotation().getRadians() - initialPose.getRotation().getRadians()) / time; - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelo); + ChassisVelocities chassisSpeeds = new ChassisVelocities(xVelocity, yVelocity, angularVelo); chassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation()); + ChassisVelocities.fromFieldRelativeSpeeds(chassisSpeeds, initialPose.getRotation()); SwerveModuleState[] swerveModuleStates = DriveConstants.KINEMATICS.toSwerveModuleStates(chassisSpeeds); diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java index 7479b04..b83303a 100644 --- a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -3,7 +3,7 @@ package frc.robot.commands.vision; import java.util.function.Supplier; import org.wpilib.math.util.MathUtil; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; @@ -29,7 +29,7 @@ public class AimAtGamePiece extends DefaultDriveCommand { } @Override - protected void drive(ChassisSpeeds speeds) { + protected void drive(ChassisVelocities speeds) { if (!VisionConstants.OBJECT_DETECTION_ENABLED) { super.drive(speeds); return; diff --git a/src/main/java/frc/robot/commands/vision/GoToPose2.java b/src/main/java/frc/robot/commands/vision/GoToPose2.java index 72c2a35..974ea57 100644 --- a/src/main/java/frc/robot/commands/vision/GoToPose2.java +++ b/src/main/java/frc/robot/commands/vision/GoToPose2.java @@ -4,7 +4,7 @@ import java.util.function.Supplier; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.command2.Command; import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; @@ -27,7 +27,7 @@ public class GoToPose2 extends Command { @Override public void initialize() { pose = poseSupplier.get(); - ChassisSpeeds v = drive.getChassisSpeeds(); + ChassisVelocities v = drive.getChassisVelocities(); vx = v.vxMetersPerSecond; vy = v.vyMetersPerSecond; 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 0bc2f0b..809c554 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -19,7 +19,7 @@ import org.wpilib.math.estimator.SwerveDrivePoseEstimator; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModulePosition; import org.wpilib.math.kinematics.SwerveModuleState; @@ -62,7 +62,7 @@ public class Drivetrain extends SubsystemBase { private SwerveSetpoint currentSetpoint = new SwerveSetpoint( - new ChassisSpeeds(), + new ChassisVelocities(), new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), @@ -280,11 +280,11 @@ public class Drivetrain extends SubsystemBase { public void drive( double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean isOpenLoop) { // rot = headingControl(rot, xSpeed, ySpeed); - ChassisSpeeds speeds = ChassisSpeeds.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); + ChassisVelocities speeds = ChassisVelocities.discretize(xSpeed, ySpeed, rot, Constants.LOOP_TIME); if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + speeds = ChassisVelocities.fromFieldRelativeSpeeds(speeds, getYaw()); } - setChassisSpeeds(speeds, isOpenLoop); + setChassisVelocities(speeds, isOpenLoop); } /** @@ -297,11 +297,11 @@ public class Drivetrain extends SubsystemBase { */ public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) { double rot = rotationController.calculate(getYaw().getRadians(), heading); - ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot); + ChassisVelocities speeds = new ChassisVelocities(xSpeed, ySpeed, rot); if (fieldRelative) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw()); + speeds = ChassisVelocities.fromFieldRelativeSpeeds(speeds, getYaw()); } - setChassisSpeeds(speeds, false); + setChassisVelocities(speeds, false); } /** @@ -480,10 +480,10 @@ public class Drivetrain extends SubsystemBase { * @param chassisSpeeds the target chassis speeds * @param isOpenLoop if open loop control should be used for the drive velocity */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, boolean isOpenLoop) { + public void setChassisVelocities(ChassisVelocities chassisSpeeds, boolean isOpenLoop) { if (DriveConstants.USE_ACTUAL_SPEED) { - SwerveSetpoint currentState = new SwerveSetpoint(getChassisSpeeds(), getModuleStates()); + SwerveSetpoint currentState = new SwerveSetpoint(getChassisVelocities(), getModuleStates()); currentSetpoint = setpointGenerator.generateSetpoint( DriveConstants.MODULE_LIMITS, @@ -589,10 +589,10 @@ public class Drivetrain extends SubsystemBase { /** * Calculates chassis speed of drivetrain using the current SwerveModuleStates * - * @return ChassisSpeeds object This is often used as an input for other methods + * @return ChassisVelocities object This is often used as an input for other methods */ - public ChassisSpeeds getChassisSpeeds() { - return DriveConstants.KINEMATICS.toChassisSpeeds(getModuleStates()); + public ChassisVelocities getChassisVelocities() { + return DriveConstants.KINEMATICS.toChassisVelocities(getModuleStates()); } /** diff --git a/src/main/java/frc/robot/util/GeomUtil.java b/src/main/java/frc/robot/util/GeomUtil.java index 4fb46d6..67e67b5 100644 --- a/src/main/java/frc/robot/util/GeomUtil.java +++ b/src/main/java/frc/robot/util/GeomUtil.java @@ -14,7 +14,7 @@ import org.wpilib.math.geometry.Transform2d; import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; /** Geometry utilities for working with translations, rotations, transforms, and poses. */ public class GeomUtil { @@ -129,12 +129,12 @@ public class GeomUtil { } /** - * Converts a ChassisSpeeds to a Twist2d by extracting two dimensions (Y and Z). chain + * Converts a ChassisVelocities to a Twist2d by extracting two dimensions (Y and Z). chain * * @param speeds The original translation * @return The resulting translation */ - public static Twist2d toTwist2d(ChassisSpeeds speeds) { + public static Twist2d toTwist2d(ChassisVelocities speeds) { return new Twist2d( speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); } diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java index f96fdc3..3dda108 100644 --- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpoint.java @@ -7,7 +7,7 @@ package frc.robot.util.SwerveStuff; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveModuleState; -public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} +public record SwerveSetpoint(ChassisVelocities chassisSpeeds, SwerveModuleState[] moduleStates) {} diff --git a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java index 1d2a3db..8039f56 100644 --- a/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java +++ b/src/main/java/frc/robot/util/SwerveStuff/SwerveSetpointGenerator.java @@ -12,7 +12,7 @@ import static frc.robot.util.EqualsUtil.*; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Twist2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.kinematics.SwerveDriveKinematics; import org.wpilib.math.kinematics.SwerveModuleState; import java.util.ArrayList; @@ -27,7 +27,7 @@ import frc.robot.util.GeomUtil; /** * "Inspired" by FRC team 254. See the license file in the root directory of this project. * - *

Takes a prior setpoint (ChassisSpeeds), a desired setpoint (from a driver, or from a path + *

Takes a prior setpoint (ChassisVelocities), a desired setpoint (from a driver, or from a path * follower), and outputs a new setpoint that respects all of the kinematic constraints on module * rotation speed and wheel velocity/acceleration. By generating a new setpoint every iteration, the * robot will converge to the desired setpoint quickly while avoiding any intermediate state that is @@ -262,7 +262,7 @@ public class SwerveSetpointGenerator { final ModuleLimits limits, double centerOfMassHeight, final SwerveSetpoint prevSetpoint, - ChassisSpeeds desiredState, + ChassisVelocities desiredState, double dt) { final Translation2d[] modules = moduleLocations; @@ -270,7 +270,7 @@ public class SwerveSetpointGenerator { // Make sure desiredState respects velocity limits. if (limits.maxDriveVelocity() > 0.0) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); - desiredState = kinematics.toChassisSpeeds(desiredModuleState); + desiredState = kinematics.toChassisVelocities(desiredModuleState); } // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so @@ -327,7 +327,7 @@ public class SwerveSetpointGenerator { // It will (likely) be faster to stop the robot, rotate the modules in place to the complement // of the desired // angle, and accelerate again. - return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisSpeeds(), dt); + return generateSetpoint(limits, centerOfMassHeight, prevSetpoint, new ChassisVelocities(), dt); } // Compute the deltas between start and goal. We can then interpolate from the start state to @@ -470,8 +470,8 @@ public class SwerveSetpointGenerator { } } - ChassisSpeeds retSpeeds = - new ChassisSpeeds( + ChassisVelocities retSpeeds = + new ChassisVelocities( prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); diff --git a/src/main/java/frc/robot/util/Vision/DetectedObject.java b/src/main/java/frc/robot/util/Vision/DetectedObject.java index f689438..cc0f81d 100644 --- a/src/main/java/frc/robot/util/Vision/DetectedObject.java +++ b/src/main/java/frc/robot/util/Vision/DetectedObject.java @@ -6,7 +6,7 @@ import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.geometry.Translation3d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.util.Units; import org.wpilib.driverstation.DriverStation.Alliance; import frc.robot.Robot; @@ -370,7 +370,7 @@ public class DetectedObject { * @return The relative angle in radians */ public double getVelocityRelativeAngle() { - ChassisSpeeds speeds = drive.getChassisSpeeds(); + ChassisVelocities speeds = drive.getChassisVelocities(); double angle = getRelativeAngle() - Math.atan2(speeds.vyMetersPerSecond, speeds.vxMetersPerSecond); 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 8ebf7fe..c50c9ff 100644 --- a/src/main/java/frc/robot/util/Vision/DriverAssist.java +++ b/src/main/java/frc/robot/util/Vision/DriverAssist.java @@ -8,7 +8,7 @@ import org.wpilib.math.util.MathUtil; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Translation2d; -import org.wpilib.math.kinematics.ChassisSpeeds; +import org.wpilib.math.kinematics.ChassisVelocities; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.trajectory.TrapezoidProfile.Constraints; import org.wpilib.math.trajectory.TrapezoidProfile.State; @@ -51,8 +51,8 @@ public class DriverAssist { * @param keepAngle True to use the angle in the pose, false to point hte robot toward the pose * @return The new speed */ - private static ChassisSpeeds calculate2( - Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) { + private static ChassisVelocities calculate2( + Drivetrain drive, ChassisVelocities driverInput, Pose2d desiredPose, boolean keepAngle) { // Do nothing if there is no pose if (desiredPose == null) { return driverInput; @@ -61,11 +61,11 @@ public class DriverAssist { // Store current states Pose2d currentPose = drive.getPose(); Rotation2d yaw = drive.getYaw(); - ChassisSpeeds driveSpeeds = drive.getChassisSpeeds(); + ChassisVelocities driveSpeeds = drive.getChassisVelocities(); driveSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( + ChassisVelocities.fromFieldRelativeSpeeds( driveSpeeds, - yaw); // Changing this does not cause problems because getChassisSpeeds() creates a new + 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); @@ -86,14 +86,14 @@ public class DriverAssist { State angleGoal = new State(rotation, 0); // Calculate ideal speeds for next frame - ChassisSpeeds goal = - new ChassisSpeeds( + ChassisVelocities goal = + new ChassisVelocities( xProfile.calculate(Constants.LOOP_TIME, xState, xGoal).velocity, yProfile.calculate(Constants.LOOP_TIME, yState, yGoal).velocity, angleProfile.calculate(Constants.LOOP_TIME, angleState, angleGoal).velocity); // Robot-relataive goal - ChassisSpeeds goalRobot = goal.times(1); - goalRobot = ChassisSpeeds.fromRobotRelativeSpeeds(goalRobot, yaw); + ChassisVelocities goalRobot = goal.times(1); + goalRobot = ChassisVelocities.fromRobotRelativeSpeeds(goalRobot, yaw); // This calculates the actual acceleration we can get // This is the only thing that needs to be robot relative @@ -104,12 +104,12 @@ public class DriverAssist { drive.getCurrSetpoint(), goalRobot, Constants.LOOP_TIME); - ChassisSpeeds nextChassisSpeed = nextSetpoint.chassisSpeeds(); - nextChassisSpeed = ChassisSpeeds.fromRobotRelativeSpeeds(nextChassisSpeed, yaw); + ChassisVelocities nextChassisSpeed = nextSetpoint.chassisSpeeds(); + nextChassisSpeed = ChassisVelocities.fromRobotRelativeSpeeds(nextChassisSpeed, yaw); // Robot relative driver inputs - ChassisSpeeds driverInputRobot = driverInput.times(1); // Copy so original doesn't change - driverInputRobot = ChassisSpeeds.fromFieldRelativeSpeeds(driverInputRobot, yaw); + ChassisVelocities driverInputRobot = driverInput.times(1); // Copy so original doesn't change + driverInputRobot = ChassisVelocities.fromFieldRelativeSpeeds(driverInputRobot, yaw); // This is the speed the driver will be able to get next frame // Both speeds need to be obtainable in 1 frame or the driver speed will always be farther away SwerveSetpoint driverSetpoint = @@ -119,11 +119,11 @@ public class DriverAssist { drive.getCurrSetpoint(), driverInputRobot, Constants.LOOP_TIME); - ChassisSpeeds driverSpeeds = driverSetpoint.chassisSpeeds(); - driverSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(driverSpeeds, yaw); + ChassisVelocities driverSpeeds = driverSetpoint.chassisSpeeds(); + driverSpeeds = ChassisVelocities.fromRobotRelativeSpeeds(driverSpeeds, yaw); // The difference between the 2 speeds - ChassisSpeeds error = nextChassisSpeed.minus(driverSpeeds); + ChassisVelocities error = nextChassisSpeed.minus(driverSpeeds); // 1.2*1.2^-distance decreases the amount it correct by as distance increases double distanceFactor = @@ -136,7 +136,7 @@ public class DriverAssist { Math.hypot(driverInput.vxMetersPerSecond, driverInput.vyMetersPerSecond); // The amount to correct by - ChassisSpeeds correction = + ChassisVelocities correction = error.times( Math.min( CORRECTION_FACTOR * distanceFactor * driverInputSpeed / DriveConstants.MAX_SPEED, @@ -164,8 +164,8 @@ public class DriverAssist { */ @SuppressWarnings( "unused") // Needed because some code might not run for some values of DRIVER_ASSIST_MODE - public static ChassisSpeeds calculate( - Drivetrain drive, ChassisSpeeds driverInput, Pose2d desiredPose, boolean keepAngle) { + public static ChassisVelocities calculate( + Drivetrain drive, ChassisVelocities driverInput, Pose2d desiredPose, boolean keepAngle) { if (VisionConstants.DRIVER_ASSIST_MODE < 2 || desiredPose == null) { return driverInput; } else if (VisionConstants.DRIVER_ASSIST_MODE == 2) { @@ -231,7 +231,7 @@ public class DriverAssist { * Math.sqrt(2 * DriveConstants.MAX_ANGULAR_ACCEL * Math.abs(rotationError)) - ROTATION_CORRECTION_FACTOR * driverInput.omegaRadiansPerSecond; return driverInput.plus( - new ChassisSpeeds( + new ChassisVelocities( correctionSpeed * Math.cos(perpendicularAngle), correctionSpeed * Math.sin(perpendicularAngle), rotationalSpeed)); -- 2.47.3