Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2025.7.2

Browse Source
This commit is contained in:
thenetworkgrinch
2025-03-19 03:45:47 +00:00
parent 4016ee2190
commit 65874df0b2
15 changed files with 195 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
swervelib/SwerveDrive.java
View File

@@ -272,7 +272,6 @@ public class SwerveDrive implements AutoCloseable
// register the drivetrain simulation // register the drivetrain simulation
SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation()); simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
} else } else
@@ -289,8 +288,11 @@ public class SwerveDrive implements AutoCloseable
getYaw(), getYaw(),
getModulePositions(), getModulePositions(),
startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight
//
zeroGyro(); // Rotation3d currentGyro = imuReadingCache.getValue();
// double offset = currentGyro.getZ() +
// startingPose.getRotation().getRadians();
// setGyroOffset(new Rotation3d(currentGyro.getX(), currentGyro.getY(), offset));
// Initialize Telemetry // Initialize Telemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
@@ -335,11 +337,13 @@ public class SwerveDrive implements AutoCloseable
} }
@Override @Override
public void close() { public void close()
{
imu.close(); imu.close();
tunerXRecommendation.close(); tunerXRecommendation.close();
for (var module : swerveModules) { for (var module : swerveModules)
{
module.close(); module.close();
} }
} }
@@ -393,7 +397,10 @@ public class SwerveDrive implements AutoCloseable
public void setOdometryPeriod(double period) public void setOdometryPeriod(double period)
{ {
odometryThread.stop(); odometryThread.stop();
SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1); if (SwerveDriveTelemetry.isSimulation)
{
SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
}
odometryThread.startPeriodic(period); odometryThread.startPeriodic(period);
} }
@@ -403,7 +410,10 @@ public class SwerveDrive implements AutoCloseable
public void stopOdometryThread() public void stopOdometryThread()
{ {
odometryThread.stop(); odometryThread.stop();
SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5); if (SwerveDriveTelemetry.isSimulation)
{
SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
}
} }
/** /**
@@ -778,7 +788,7 @@ public class SwerveDrive implements AutoCloseable
{ {
module.applyStateOptimizations(states[module.moduleNumber]); module.applyStateOptimizations(states[module.moduleNumber]);
module.applyAntiJitter(states[module.moduleNumber], false); module.applyAntiJitter(states[module.moduleNumber], false);
// from the module configuration, obtain necessary information to calculate feed-forward // from the module configuration, obtain necessary information to calculate feed-forward
// Warning: Will not work well if motor is not what we are expecting. // Warning: Will not work well if motor is not what we are expecting.
// Warning: Should replace module.getDriveMotor().simMotor with expected motor type first. // Warning: Should replace module.getDriveMotor().simMotor with expected motor type first.
@@ -898,7 +908,7 @@ public class SwerveDrive implements AutoCloseable
* method. However, if either gyro angle or module position is reset, this must be called in order for odometry to * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
* keep working. * keep working.
* *
* @param pose The pose to set the odometry to * @param pose The pose to set the odometry to. Field relative, blue-origin where 0deg is facing towards RED alliance.
*/ */
public void resetOdometry(Pose2d pose) public void resetOdometry(Pose2d pose)
{ {
@@ -987,7 +997,7 @@ public class SwerveDrive implements AutoCloseable
} }
/** /**
* Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station).
*/ */
public void zeroGyro() public void zeroGyro()
{ {

243
swervelib/SwerveInputStream.java
View File

@@ -1,11 +1,16 @@
package swervelib; package swervelib;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController;
@@ -57,99 +62,95 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/** /**
* Rotation supplier as angular velocity. * Rotation supplier as angular velocity.
*/ */
private Optional<DoubleSupplier> controllerOmega = Optional.empty(); private Optional<DoubleSupplier> controllerOmega = Optional.empty();
/** /**
* Controller supplier as heading. * Controller supplier as heading.
*/ */
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty(); private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
/** /**
* Controller supplier as heading. * Controller supplier as heading.
*/ */
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty(); private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
/** /**
* Axis deadband for the controller. * Axis deadband for the controller.
*/ */
private Optional<Double> axisDeadband = Optional.empty(); private Optional<Double> axisDeadband = Optional.empty();
/** /**
* Translational axis scalar value, should be between (0, 1]. * Translational axis scalar value, should be between (0, 1].
*/ */
private Optional<Double> translationAxisScale = Optional.empty(); private Optional<Double> translationAxisScale = Optional.empty();
/** /**
* Angular velocity axis scalar value, should be between (0, 1] * Angular velocity axis scalar value, should be between (0, 1]
*/ */
private Optional<Double> omegaAxisScale = Optional.empty(); private Optional<Double> omegaAxisScale = Optional.empty();
/** /**
* Target to aim at. * Target to aim at.
*/ */
private Optional<Pose2d> aimTarget = Optional.empty(); private Optional<Pose2d> aimTarget = Optional.empty();
/** /**
* Target {@link Supplier<Pose2d>} to drive towards when driveToPose is enabled. * Target {@link Supplier<Pose2d>} to drive towards when driveToPose is enabled.
*/ */
private Optional<Supplier<Pose2d>> driveToPose = Optional.empty(); private Optional<Supplier<Pose2d>> driveToPose = Optional.empty();
/** /**
* {@link ProfiledPIDController} for the X translation while driving to a pose. Units are m/s * {@link ProfiledPIDController} for the translation while driving to a pose. Units are m/s
*/ */
private Optional<ProfiledPIDController> driveToPoseXPIDController = Optional.empty(); private Optional<ProfiledPIDController> driveToPoseTranslationPIDController = Optional.empty();
/**
* {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s
*/
private Optional<ProfiledPIDController> driveToPoseYPIDController = Optional.empty();
/** /**
* {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s * {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s
*/ */
private Optional<ProfiledPIDController> driveToPoseOmegaPIDController = Optional.empty(); private Optional<ProfiledPIDController> driveToPoseOmegaPIDController = Optional.empty();
/** /**
* Output {@link ChassisSpeeds} based on heading while this is True. * Output {@link ChassisSpeeds} based on heading while this is True.
*/ */
private Optional<BooleanSupplier> headingEnabled = Optional.empty(); private Optional<BooleanSupplier> headingEnabled = Optional.empty();
/** /**
* Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY} * Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY}
*/ */
private Optional<Rotation2d> lockedHeading = Optional.empty(); private Optional<Rotation2d> lockedHeading = Optional.empty();
/** /**
* Output {@link ChassisSpeeds} based on aim while this is True. * Output {@link ChassisSpeeds} based on aim while this is True.
*/ */
private Optional<BooleanSupplier> aimEnabled = Optional.empty(); private Optional<BooleanSupplier> aimEnabled = Optional.empty();
/** /**
* Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}. * Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}.
*/ */
private Optional<BooleanSupplier> driveToPoseEnabled = Optional.empty(); private Optional<BooleanSupplier> driveToPoseEnabled = Optional.empty();
/** /**
* Maintain current heading and drive without rotating, ideally. * Maintain current heading and drive without rotating, ideally.
*/ */
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty(); private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
/** /**
* Cube the translation magnitude from the controller. * Cube the translation magnitude from the controller.
*/ */
private Optional<BooleanSupplier> translationCube = Optional.empty(); private Optional<BooleanSupplier> translationCube = Optional.empty();
/** /**
* Cube the angular velocity axis from the controller. * Cube the angular velocity axis from the controller.
*/ */
private Optional<BooleanSupplier> omegaCube = Optional.empty(); private Optional<BooleanSupplier> omegaCube = Optional.empty();
/** /**
* Robot relative oriented output expected. * Robot relative oriented output expected.
*/ */
private Optional<BooleanSupplier> robotRelative = Optional.empty(); private Optional<BooleanSupplier> robotRelative = Optional.empty();
/** /**
* Field oriented chassis output is relative to your current alliance. * Field oriented chassis output is relative to your current alliance.
*/ */
private Optional<BooleanSupplier> allianceRelative = Optional.empty(); private Optional<BooleanSupplier> allianceRelative = Optional.empty();
/** /**
* Heading offset enable state. * Heading offset enable state.
*/ */
private Optional<BooleanSupplier> headingOffsetEnabled = Optional.empty(); private Optional<BooleanSupplier> translationHeadingOffsetEnabled = Optional.empty();
/** /**
* Heading offset to apply during heading based control. * Heading offset to apply during heading based control.
*/ */
private Optional<Rotation2d> headingOffset = Optional.empty(); private Optional<Rotation2d> translationHeadingOffset = Optional.empty();
/** /**
* {@link SwerveController} for simple control over heading. * {@link SwerveController} for simple control over heading.
*/ */
private SwerveController swerveController = null; private SwerveController swerveController = null;
/** /**
* Current {@link SwerveInputMode} to use. * Current {@link SwerveInputMode} to use.
*/ */
private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY; private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY;
/** /**
@@ -225,8 +226,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
newStream.translationAxisScale = translationAxisScale; newStream.translationAxisScale = translationAxisScale;
newStream.omegaAxisScale = omegaAxisScale; newStream.omegaAxisScale = omegaAxisScale;
newStream.driveToPose = driveToPose; newStream.driveToPose = driveToPose;
newStream.driveToPoseXPIDController = driveToPoseXPIDController; newStream.driveToPoseTranslationPIDController = driveToPoseTranslationPIDController;
newStream.driveToPoseYPIDController = driveToPoseYPIDController;
newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController; newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController;
newStream.aimTarget = aimTarget; newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled; newStream.headingEnabled = headingEnabled;
@@ -240,8 +240,8 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
newStream.translationCube = translationCube; newStream.translationCube = translationCube;
newStream.robotRelative = robotRelative; newStream.robotRelative = robotRelative;
newStream.allianceRelative = allianceRelative; newStream.allianceRelative = allianceRelative;
newStream.headingOffsetEnabled = headingOffsetEnabled; newStream.translationHeadingOffsetEnabled = translationHeadingOffsetEnabled;
newStream.headingOffset = headingOffset; newStream.translationHeadingOffset = translationHeadingOffset;
return newStream; return newStream;
} }
@@ -273,37 +273,23 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* Drive to a given pose with the provided {@link ProfiledPIDController}s * Drive to a given pose with the provided {@link ProfiledPIDController}s
* *
* @param pose {@link Supplier<Pose2d>} for ease of use. * @param pose {@link Supplier<Pose2d>} for ease of use.
* @param xPIDController PID controller for the X axis, units are m/s. * @param xPIDController PID controller for the translational axis, units are m/s.
* @param yPIDController PID controller for the Y axis, units are m/s.
* @param omegaPIDController PID Controller for rotational axis, units are rad/s. * @param omegaPIDController PID Controller for rotational axis, units are rad/s.
* @return self * @return self
*/ */
public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController, public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController,
ProfiledPIDController yPIDController, ProfiledPIDController omegaPIDController) ProfiledPIDController omegaPIDController)
{ {
omegaPIDController.reset(swerveDrive.getPose().getRotation().getRadians());
xPIDController.reset(swerveDrive.getPose().getTranslation().getDistance(pose.get().getTranslation()));
omegaPIDController.enableContinuousInput(-Math.PI, Math.PI);
xPIDController.setGoal(new State(0, 0));
driveToPose = Optional.of(pose); driveToPose = Optional.of(pose);
driveToPoseXPIDController = Optional.of(xPIDController); driveToPoseTranslationPIDController = Optional.of(xPIDController);
driveToPoseYPIDController = Optional.of(yPIDController);
driveToPoseOmegaPIDController = Optional.of(omegaPIDController); driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
return this; return this;
} }
/**
* Drive to a given pose with the provided {@link ProfiledPIDController}s
*
* @param pose {@link Supplier<Pose2d>} for ease of use.
* @param translation PID controller for the X and Y axis, units are m/s.
* @param rotation PID Controller for rotational axis, units are rad/s.
* @return self
*/
public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController translation,
ProfiledPIDController rotation)
{
return driveToPose(pose, translation, new ProfiledPIDController(translation.getP(),
translation.getI(),
translation.getD(),
translation.getConstraints()), rotation);
}
/** /**
* Enable driving to the target pose. * Enable driving to the target pose.
@@ -326,6 +312,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
public SwerveInputStream driveToPoseEnabled(boolean enabled) public SwerveInputStream driveToPoseEnabled(boolean enabled)
{ {
driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty(); driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
Pose2d swervePose = swerveDrive.getPose();
// driveToPoseXPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getX()));
// driveToPoseYPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getY()));
// driveToPoseOmegaPIDController.ifPresent(profiledPIDController -> profiledPIDController.reset(swervePose.getRotation()
// .getRadians()));
return this; return this;
} }
@@ -336,9 +327,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param enabled Enable state * @param enabled Enable state
* @return self * @return self
*/ */
public SwerveInputStream headingOffset(BooleanSupplier enabled) public SwerveInputStream translationHeadingOffset(BooleanSupplier enabled)
{ {
headingOffsetEnabled = Optional.of(enabled); translationHeadingOffsetEnabled = Optional.of(enabled);
return this; return this;
} }
@@ -348,9 +339,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param enabled Enable state * @param enabled Enable state
* @return self * @return self
*/ */
public SwerveInputStream headingOffset(boolean enabled) public SwerveInputStream translationHeadingOffset(boolean enabled)
{ {
headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty(); translationHeadingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this; return this;
} }
@@ -360,9 +351,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param angle {@link Rotation2d} offset to apply * @param angle {@link Rotation2d} offset to apply
* @return self * @return self
*/ */
public SwerveInputStream headingOffset(Rotation2d angle) public SwerveInputStream translationHeadingOffset(Rotation2d angle)
{ {
headingOffset = Optional.of(angle); translationHeadingOffset = Optional.of(angle);
return this; return this;
} }
@@ -613,11 +604,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{ {
if (driveToPose.isPresent()) if (driveToPose.isPresent())
{ {
if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() && if (driveToPoseOmegaPIDController.isPresent() && driveToPoseTranslationPIDController.isPresent())
driveToPoseYPIDController.isPresent())
{ {
return SwerveInputMode.DRIVE_TO_POSE; return SwerveInputMode.DRIVE_TO_POSE;
} }
System.out.println("Drive to pose present");
DriverStation.reportError("Drive to pose not supplied with pid controllers.", false); DriverStation.reportError("Drive to pose not supplied with pid controllers.", false);
} }
DriverStation.reportError("Drive to pose enabled without supplier present.", false); DriverStation.reportError("Drive to pose enabled without supplier present.", false);
@@ -671,11 +662,15 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.empty(); lockedHeading = Optional.empty();
break; break;
} }
case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE -> case ANGULAR_VELOCITY, HEADING, AIM ->
{ {
// Do nothing // Do nothing
break; break;
} }
case DRIVE_TO_POSE ->
{
break;
}
} }
// Transitioning to new mode // Transitioning to new mode
@@ -686,7 +681,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.of(swerveDrive.getOdometryHeading()); lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
break; break;
} }
case ANGULAR_VELOCITY, DRIVE_TO_POSE -> case ANGULAR_VELOCITY ->
{ {
if (swerveDrive.headingCorrection) if (swerveDrive.headingCorrection)
{ {
@@ -699,6 +694,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
// Do nothing // Do nothing
break; break;
} }
case DRIVE_TO_POSE ->
{
if (swerveDrive.headingCorrection)
{
swerveDrive.setHeadingCorrection(false);
}
}
} }
} }
@@ -807,6 +809,10 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{ {
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean()) if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
{ {
if (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean())
{
return fieldRelativeTranslation;
}
throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!"); throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
} }
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
@@ -818,43 +824,49 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
} }
/** /**
* Apply alliance aware translation which flips the {@link Rotation2d} if the robot is on the Blue alliance. * Adds offset to translation if one is set.
* *
* @param fieldRelativeRotation Field-relative {@link Rotation2d} to flip. * @param speeds {@link ChassisSpeeds} to offset
* @return Alliance-oriented {@link Rotation2d} * @return Offsetted {@link ChassisSpeeds}
*/ */
private Rotation2d applyAllianceAwareRotation(Rotation2d fieldRelativeRotation) private ChassisSpeeds applyTranslationHeadingOffset(ChassisSpeeds speeds)
{ {
if (allianceRelative.isPresent() && allianceRelative.get().getAsBoolean()) if (translationHeadingOffsetEnabled.isPresent() && translationHeadingOffsetEnabled.get().getAsBoolean())
{ {
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean()) if (translationHeadingOffset.isPresent())
{ {
throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!"); Translation2d speedsTranslation = new Translation2d(speeds.vxMetersPerSecond,
} speeds.vyMetersPerSecond).rotateBy(translationHeadingOffset.get());
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) return new ChassisSpeeds(speedsTranslation.getX(), speedsTranslation.getY(), speeds.omegaRadiansPerSecond);
{
return fieldRelativeRotation.rotateBy(Rotation2d.k180deg);
} }
} }
return fieldRelativeRotation; return speeds;
} }
/** /**
* Adds offset to rotation if one is set. * When the {@link SwerveInputStream} is in {@link SwerveInputMode#DRIVE_TO_POSE} this function will return if the
* robot is at the desired pose within the defined tolerance.
* *
* @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset * @param toleranceMeters Tolerance in meters.
* @return Offsetted {@link Rotation2d} * @return At target pose, true if current mode is not {@link SwerveInputMode#DRIVE_TO_POSE} and no pose supplier has
* been given.
*/ */
private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation) public boolean atTargetPose(double toleranceMeters)
{ {
if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean()) if (currentMode != SwerveInputMode.DRIVE_TO_POSE)
{ {
if (headingOffset.isPresent()) DriverStation.reportError("SwerveInputStream.atTargetPose called while not set to DriveToPose.", false);
if (!driveToPose.isPresent())
{ {
return fieldRelativeRotation.rotateBy(headingOffset.get()); return true;
} }
} }
return fieldRelativeRotation; if (driveToPose.isPresent())
{
Pose2d targetPose = driveToPose.get().get();
return swerveDrive.getPose().getTranslation().getDistance(targetPose.getTranslation()) <= toleranceMeters;
}
return true;
} }
/** /**
@@ -886,7 +898,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{ {
swerveController = swerveDrive.getSwerveController(); swerveController = swerveDrive.getSwerveController();
} }
switch (newMode) switch (newMode)
{ {
case TRANSLATION_ONLY -> case TRANSLATION_ONLY ->
@@ -907,14 +918,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
case HEADING -> case HEADING ->
{ {
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(), omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
applyHeadingOffset( Rotation2d.fromRadians(
applyAllianceAwareRotation( swerveController.getJoystickAngle(
Rotation2d.fromRadians( controllerHeadingX.get()
swerveController.getJoystickAngle( .getAsDouble(),
controllerHeadingX.get() controllerHeadingY.get()
.getAsDouble(), .getAsDouble()))
controllerHeadingY.get() .getRadians());
.getAsDouble())))).getRadians());
// Prevent rotation if controller heading inputs are not past axisDeadband // Prevent rotation if controller heading inputs are not past axisDeadband
if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) < if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) <
@@ -922,7 +932,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{ {
omegaRadiansPerSecond = 0; omegaRadiansPerSecond = 0;
} }
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break; break;
} }
@@ -937,20 +946,50 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
} }
case DRIVE_TO_POSE -> case DRIVE_TO_POSE ->
{ {
Pose2d target = driveToPose.get().get(); // Written by team 8865!
Pose2d pose = swerveDrive.getPose(); ProfiledPIDController translationPIDController = driveToPoseTranslationPIDController.get();
omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation() ProfiledPIDController rotationPIDController = driveToPoseOmegaPIDController.get();
.getRadians(), Pose2d swervePoseSetpoint = driveToPose.get().get();
target.getRotation().getRadians()); Pose2d robotPose = swerveDrive.getPose();
speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()), Vector<N2> robotVec = robotPose.getTranslation().toVector();
driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()), Vector<N2> targetPoseRelativeToRobotPose = swervePoseSetpoint.getTranslation().toVector().minus(
omegaRadiansPerSecond); robotVec);
double distanceFromTarget = targetPoseRelativeToRobotPose.norm();
Vector<N2> traversalVector = new Vector(Nat.N2());
traversalVector.set(0, 0, targetPoseRelativeToRobotPose.get(0, 0));
traversalVector.set(1, 0, targetPoseRelativeToRobotPose.get(1, 0));
traversalVector = traversalVector.unit()
.times(-translationPIDController.calculate(distanceFromTarget, 0));
Vector<N2> robotForwardVec = robotPose.transformBy(new Transform2d(1, 0, new Rotation2d())).getTranslation()
.toVector().minus(robotVec);
Vector<N2> robotLateralVec = robotPose.transformBy(new Transform2d(0, 1, new Rotation2d())).getTranslation()
.toVector().minus(robotVec);
currentMode = newMode;
speeds = ChassisSpeeds.fromRobotRelativeSpeeds(new ChassisSpeeds(
robotForwardVec.norm() * traversalVector.dot(robotForwardVec),
robotLateralVec.norm() * traversalVector.dot(robotLateralVec),
rotationPIDController.calculate(robotPose.getRotation().getRadians(),
swervePoseSetpoint.getRotation().getRadians())),
swerveDrive.getOdometryHeading());
double lerpDistance = robotPose.getTranslation().plus(new Translation2d(speeds.vxMetersPerSecond,
vyMetersPerSecond).times(0.02))
.getDistance(swervePoseSetpoint.getTranslation());
// Filter out incorrect ChassisSpeeds.
if (lerpDistance > distanceFromTarget)
{
speeds = new ChassisSpeeds(0, 0, 0);
}
return speeds;
} }
} }
currentMode = newMode; currentMode = newMode;
return applyRobotRelativeTranslation(speeds); return applyTranslationHeadingOffset(applyRobotRelativeTranslation(speeds));
} }
/** /**

12
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -206,6 +206,18 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
@Override @Override
public boolean setAbsoluteEncoderOffset(double offset) public boolean setAbsoluteEncoderOffset(double offset)
{ {
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
return true;
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
return false; return false;
} }

2
swervelib/imu/ADIS16448Swerve.java
View File

@@ -107,7 +107,7 @@ public class ADIS16448Swerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/ADIS16470Swerve.java
View File

@@ -107,7 +107,7 @@ public class ADIS16470Swerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/ADXRS450Swerve.java
View File

@@ -105,7 +105,7 @@ public class ADXRS450Swerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -112,7 +112,7 @@ public class AnalogGyroSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/CanandgyroSwerve.java
View File

@@ -109,7 +109,7 @@ public class CanandgyroSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

23
swervelib/imu/NavXSwerve.java
View File

@@ -20,7 +20,7 @@ public class NavXSwerve extends SwerveIMU
/** /**
* Mutable {@link MutAngularVelocity} for readings. * Mutable {@link MutAngularVelocity} for readings.
*/ */
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/** /**
* NavX IMU. * NavX IMU.
*/ */
@@ -28,15 +28,15 @@ public class NavXSwerve extends SwerveIMU
/** /**
* Offset for the NavX. * Offset for the NavX.
*/ */
private Rotation3d offset = new Rotation3d(); private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/** /**
* An {@link Alert} for if there is an error instantiating the NavX. * An {@link Alert} for if there is an error instantiating the NavX.
*/ */
private Alert navXError; private Alert navXError;
/**
* Inversion state of the {@link AHRS}.
*/
private boolean inverted = false;
/** /**
* Constructor for the NavX({@link AHRS}) swerve. * Constructor for the NavX({@link AHRS}) swerve.
@@ -61,7 +61,8 @@ public class NavXSwerve extends SwerveIMU
} }
@Override @Override
public void close() { public void close()
{
imu.close(); imu.close();
} }
@@ -101,8 +102,8 @@ public class NavXSwerve extends SwerveIMU
*/ */
public void setInverted(boolean invertIMU) public void setInverted(boolean invertIMU)
{ {
invertedIMU = invertIMU; inverted = invertIMU;
setOffset(getRawRotation3d()); // setOffset(getRawRotation3d());
} }
/** /**
@@ -113,7 +114,7 @@ public class NavXSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRawRotation3d() public Rotation3d getRawRotation3d()
{ {
return invertedIMU ? imu.getRotation3d().unaryMinus() : imu.getRotation3d(); return inverted ? imu.getRotation3d().unaryMinus() : imu.getRotation3d();
} }
/** /**
@@ -124,7 +125,7 @@ public class NavXSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/Pigeon2Swerve.java
View File

@@ -152,7 +152,7 @@ public class Pigeon2Swerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }

2
swervelib/imu/PigeonSwerve.java
View File

@@ -111,7 +111,7 @@ public class PigeonSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

2
swervelib/imu/PigeonViaTalonSRXSwerve.java
View File

@@ -121,7 +121,7 @@ public class PigeonViaTalonSRXSwerve extends SwerveIMU
@Override @Override
public Rotation3d getRotation3d() public Rotation3d getRotation3d()
{ {
return getRawRotation3d().minus(offset); return getRawRotation3d().rotateBy(offset.unaryMinus());
} }
/** /**

6
swervelib/motors/SparkFlexSwerve.java
View File

@@ -155,7 +155,7 @@ public class SparkFlexSwerve extends SwerveMotor
{ {
if (!DriverStation.isDisabled()) if (!DriverStation.isDisabled())
{ {
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled."); DriverStation.reportWarning("Configuration changes cannot be applied while the robot is enabled.", false);
} }
cfg.apply(cfgGiven); cfg.apply(cfgGiven);
configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -392,10 +392,6 @@ public class SparkFlexSwerve extends SwerveMotor
@Override @Override
public void burnFlash() public void burnFlash()
{ {
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkFlex(() -> { configureSparkFlex(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}); });

8
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -214,10 +214,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
*/ */
public void updateConfig(SparkMaxConfig cfgGiven) public void updateConfig(SparkMaxConfig cfgGiven)
{ {
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
}
cfg.apply(cfgGiven); cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
} }
@@ -466,10 +462,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override @Override
public void burnFlash() public void burnFlash()
{ {
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> { configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}); });

6
swervelib/motors/SparkMaxSwerve.java
View File

@@ -148,7 +148,7 @@ public class SparkMaxSwerve extends SwerveMotor
{ {
if (!DriverStation.isDisabled()) if (!DriverStation.isDisabled())
{ {
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled."); DriverStation.reportWarning("Configuration changes cannot be applied while the robot is enabled.", false);
} }
cfg.apply(cfgGiven); cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -391,10 +391,6 @@ public class SparkMaxSwerve extends SwerveMotor
@Override @Override
public void burnFlash() public void burnFlash()
{ {
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> { configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
}); });