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
SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
} else
@@ -289,8 +288,11 @@ public class SwerveDrive implements AutoCloseable
getYaw(),
getModulePositions(),
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
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
@@ -335,11 +337,13 @@ public class SwerveDrive implements AutoCloseable
}
@Override
public void close() {
public void close()
{
imu.close();
tunerXRecommendation.close();
for (var module : swerveModules) {
for (var module : swerveModules)
{
module.close();
}
}
@@ -393,7 +397,10 @@ public class SwerveDrive implements AutoCloseable
public void setOdometryPeriod(double period)
{
odometryThread.stop();
SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
if (SwerveDriveTelemetry.isSimulation)
{
SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
}
odometryThread.startPeriodic(period);
}
@@ -403,7 +410,10 @@ public class SwerveDrive implements AutoCloseable
public void stopOdometryThread()
{
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.applyAntiJitter(states[module.moduleNumber], false);
// 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: 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
* 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)
{
@@ -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()
{

243
swervelib/SwerveInputStream.java
View File

@@ -1,11 +1,16 @@
package swervelib;
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.geometry.Pose2d;
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.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.Alliance;
import edu.wpi.first.wpilibj.XboxController;
@@ -57,99 +62,95 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/**
* Rotation supplier as angular velocity.
*/
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
/**
* 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].
*/
private Optional<Double> translationAxisScale = Optional.empty();
private Optional<Double> translationAxisScale = Optional.empty();
/**
* 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.
*/
private Optional<Pose2d> aimTarget = Optional.empty();
private Optional<Pose2d> aimTarget = Optional.empty();
/**
* 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();
/**
* {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s
*/
private Optional<ProfiledPIDController> driveToPoseYPIDController = Optional.empty();
private Optional<ProfiledPIDController> driveToPoseTranslationPIDController = Optional.empty();
/**
* {@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.
*/
private Optional<BooleanSupplier> headingEnabled = Optional.empty();
private Optional<BooleanSupplier> headingEnabled = Optional.empty();
/**
* 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.
*/
private Optional<BooleanSupplier> aimEnabled = Optional.empty();
private Optional<BooleanSupplier> aimEnabled = Optional.empty();
/**
* 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.
*/
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
/**
* 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.
*/
private Optional<BooleanSupplier> omegaCube = Optional.empty();
private Optional<BooleanSupplier> omegaCube = Optional.empty();
/**
* 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.
*/
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
/**
* Heading offset enable state.
*/
private Optional<BooleanSupplier> headingOffsetEnabled = Optional.empty();
private Optional<BooleanSupplier> translationHeadingOffsetEnabled = Optional.empty();
/**
* 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.
*/
private SwerveController swerveController = null;
private SwerveController swerveController = null;
/**
* 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.omegaAxisScale = omegaAxisScale;
newStream.driveToPose = driveToPose;
newStream.driveToPoseXPIDController = driveToPoseXPIDController;
newStream.driveToPoseYPIDController = driveToPoseYPIDController;
newStream.driveToPoseTranslationPIDController = driveToPoseTranslationPIDController;
newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController;
newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled;
@@ -240,8 +240,8 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
newStream.translationCube = translationCube;
newStream.robotRelative = robotRelative;
newStream.allianceRelative = allianceRelative;
newStream.headingOffsetEnabled = headingOffsetEnabled;
newStream.headingOffset = headingOffset;
newStream.translationHeadingOffsetEnabled = translationHeadingOffsetEnabled;
newStream.translationHeadingOffset = translationHeadingOffset;
return newStream;
}
@@ -273,37 +273,23 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* Drive to a given pose with the provided {@link ProfiledPIDController}s
*
* @param pose {@link Supplier<Pose2d>} for ease of use.
* @param xPIDController PID controller for the X axis, units are m/s.
* @param yPIDController PID controller for the Y axis, units are m/s.
* @param xPIDController PID controller for the translational axis, units are m/s.
* @param omegaPIDController PID Controller for rotational axis, units are rad/s.
* @return self
*/
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);
driveToPoseXPIDController = Optional.of(xPIDController);
driveToPoseYPIDController = Optional.of(yPIDController);
driveToPoseTranslationPIDController = Optional.of(xPIDController);
driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
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.
@@ -326,6 +312,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
public SwerveInputStream driveToPoseEnabled(boolean enabled)
{
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;
}
@@ -336,9 +327,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param enabled Enable state
* @return self
*/
public SwerveInputStream headingOffset(BooleanSupplier enabled)
public SwerveInputStream translationHeadingOffset(BooleanSupplier enabled)
{
headingOffsetEnabled = Optional.of(enabled);
translationHeadingOffsetEnabled = Optional.of(enabled);
return this;
}
@@ -348,9 +339,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param enabled Enable state
* @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;
}
@@ -360,9 +351,9 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* @param angle {@link Rotation2d} offset to apply
* @return self
*/
public SwerveInputStream headingOffset(Rotation2d angle)
public SwerveInputStream translationHeadingOffset(Rotation2d angle)
{
headingOffset = Optional.of(angle);
translationHeadingOffset = Optional.of(angle);
return this;
}
@@ -613,11 +604,11 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{
if (driveToPose.isPresent())
{
if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() &&
driveToPoseYPIDController.isPresent())
if (driveToPoseOmegaPIDController.isPresent() && driveToPoseTranslationPIDController.isPresent())
{
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 enabled without supplier present.", false);
@@ -671,11 +662,15 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.empty();
break;
}
case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE ->
case ANGULAR_VELOCITY, HEADING, AIM ->
{
// Do nothing
break;
}
case DRIVE_TO_POSE ->
{
break;
}
}
// Transitioning to new mode
@@ -686,7 +681,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
break;
}
case ANGULAR_VELOCITY, DRIVE_TO_POSE ->
case ANGULAR_VELOCITY ->
{
if (swerveDrive.headingCorrection)
{
@@ -699,6 +694,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
// Do nothing
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 (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean())
{
return fieldRelativeTranslation;
}
throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
}
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.
* @return Alliance-oriented {@link Rotation2d}
* @param speeds {@link ChassisSpeeds} to offset
* @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!");
}
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
{
return fieldRelativeRotation.rotateBy(Rotation2d.k180deg);
Translation2d speedsTranslation = new Translation2d(speeds.vxMetersPerSecond,
speeds.vyMetersPerSecond).rotateBy(translationHeadingOffset.get());
return new ChassisSpeeds(speedsTranslation.getX(), speedsTranslation.getY(), speeds.omegaRadiansPerSecond);
}
}
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
* @return Offsetted {@link Rotation2d}
* @param toleranceMeters Tolerance in meters.
* @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();
}
switch (newMode)
{
case TRANSLATION_ONLY ->
@@ -907,14 +918,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
case HEADING ->
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
applyHeadingOffset(
applyAllianceAwareRotation(
Rotation2d.fromRadians(
swerveController.getJoystickAngle(
controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble())))).getRadians());
Rotation2d.fromRadians(
swerveController.getJoystickAngle(
controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble()))
.getRadians());
// Prevent rotation if controller heading inputs are not past axisDeadband
if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) <
@@ -922,7 +932,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{
omegaRadiansPerSecond = 0;
}
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
@@ -937,20 +946,50 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
}
case DRIVE_TO_POSE ->
{
Pose2d target = driveToPose.get().get();
Pose2d pose = swerveDrive.getPose();
omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation()
.getRadians(),
target.getRotation().getRadians());
speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()),
driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()),
omegaRadiansPerSecond);
// Written by team 8865!
ProfiledPIDController translationPIDController = driveToPoseTranslationPIDController.get();
ProfiledPIDController rotationPIDController = driveToPoseOmegaPIDController.get();
Pose2d swervePoseSetpoint = driveToPose.get().get();
Pose2d robotPose = swerveDrive.getPose();
Vector<N2> robotVec = robotPose.getTranslation().toVector();
Vector<N2> targetPoseRelativeToRobotPose = swervePoseSetpoint.getTranslation().toVector().minus(
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;
return applyRobotRelativeTranslation(speeds);
return applyTranslationHeadingOffset(applyRobotRelativeTranslation(speeds));
}
/**

12
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -206,6 +206,18 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
@Override
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;
}

2
swervelib/imu/ADIS16448Swerve.java
View File

@@ -107,7 +107,7 @@ public class ADIS16448Swerve extends SwerveIMU
@Override
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
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
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
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
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.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* NavX IMU.
*/
@@ -28,15 +28,15 @@ public class NavXSwerve extends SwerveIMU
/**
* Offset for the NavX.
*/
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private Rotation3d offset = new Rotation3d();
/**
* An {@link Alert} for if there is an error instantiating the NavX.
*/
private Alert navXError;
/**
* Inversion state of the {@link AHRS}.
*/
private boolean inverted = false;
/**
* Constructor for the NavX({@link AHRS}) swerve.
@@ -61,7 +61,8 @@ public class NavXSwerve extends SwerveIMU
}
@Override
public void close() {
public void close()
{
imu.close();
}
@@ -101,8 +102,8 @@ public class NavXSwerve extends SwerveIMU
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
setOffset(getRawRotation3d());
inverted = invertIMU;
// setOffset(getRawRotation3d());
}
/**
@@ -113,7 +114,7 @@ public class NavXSwerve extends SwerveIMU
@Override
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
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
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
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
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())
{
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);
configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -392,10 +392,6 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkFlex(() -> {
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)
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
}
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
}
@@ -466,10 +462,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> {
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())
{
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);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
@@ -391,10 +391,6 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});