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

Updated to 2024.4.6.1

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-22 15:11:18 -06:00
parent 7c76cffc78
commit a20a6b83af
120 changed files with 923 additions and 603 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
swervelib/SwerveDrive.java
View File

@@ -1,7 +1,6 @@
package swervelib;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -72,9 +71,12 @@ public class SwerveDrive
*/
private final Lock odometryLock = new ReentrantLock();
/**
* Deadband for speeds in heading correction.
* Alert to recommend Tuner X if the configuration is compatible.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.WARNING);
/**
* Field object.
*/
@@ -83,26 +85,6 @@ public class SwerveDrive
* Swerve controller for controlling heading of the robot.
*/
public SwerveController swerveController;
/**
* Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of
* rotation)
*/
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
/**
* The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more
* points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)}
* with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get
* vision accurate to inches instead of feet.
*/
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
@@ -112,6 +94,10 @@ public class SwerveDrive
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
/**
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Whether heading correction PID is currently active.
*/
@@ -144,13 +130,6 @@ public class SwerveDrive
* Maximum speed of the robot in meters per second.
*/
private double maxSpeedMPS;
/**
* Alert to recommend Tuner X if the configuration is compatible.
*/
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.WARNING);
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -194,9 +173,8 @@ public class SwerveDrive
kinematics,
getYaw(),
getModulePositions(),
new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
stateStdDevs,
visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight
new Pose2d(new Translation2d(0, 0),
Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
@@ -308,6 +286,16 @@ public class SwerveDrive
}
}
/**
* Fetch the latest odometry heading, should be trusted over {@link SwerveDrive#getYaw()}.
*
* @return {@link Rotation2d} of the robot heading.
*/
public Rotation2d getOdometryHeading()
{
return swerveDrivePoseEstimator.getEstimatedPosition().getRotation();
}
/**
* Set the heading correction capabilities of YAGSL.
*
@@ -337,7 +325,7 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity);
}
@@ -349,7 +337,7 @@ public class SwerveDrive
*/
public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
{
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getYaw());
ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
drive(fieldOrientedVelocity, centerOfRotationMeters);
}
@@ -401,7 +389,7 @@ public class SwerveDrive
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getYaw())
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
drive(velocity, isOpenLoop, centerOfRotationMeters);
@@ -430,7 +418,7 @@ public class SwerveDrive
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getYaw())
translation.getX(), translation.getY(), rotation, getOdometryHeading())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
drive(velocity, isOpenLoop, new Translation2d());
@@ -466,11 +454,11 @@ public class SwerveDrive
{
if (!correctionEnabled)
{
lastHeadingRadians = getYaw().getRadians();
lastHeadingRadians = getOdometryHeading().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
} else
{
correctionEnabled = false;
@@ -623,7 +611,7 @@ public class SwerveDrive
// angle
// given as the robot angle reverses the direction of rotation, and the conversion is reversed.
return ChassisSpeeds.fromFieldRelativeSpeeds(
kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
}
/**
@@ -646,7 +634,7 @@ public class SwerveDrive
public void resetOdometry(Pose2d pose)
{
odometryLock.lock();
swerveDrivePoseEstimator.resetPosition(pose.getRotation(), getModulePositions(), pose);
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
odometryLock.unlock();
kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
}
@@ -680,8 +668,7 @@ public class SwerveDrive
}
/**
* Gets the current module positions (azimuth and wheel position (meters)). Inverts the distance from each module if
* {@link #invertOdometry} is true.
* Gets the current module positions (azimuth and wheel position (meters)).
*
* @return A list of SwerveModulePositions containg the current module positions
*/
@@ -692,10 +679,6 @@ public class SwerveDrive
for (SwerveModule module : swerveModules)
{
positions[module.moduleNumber] = module.getPosition();
if (invertOdometry)
{
positions[module.moduleNumber].distanceMeters *= -1;
}
}
return positions;
}
@@ -729,9 +712,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getZ())
: Rotation2d.fromRadians(imu.getRotation3d().getZ());
return Rotation2d.fromRadians(imu.getRotation3d().getZ());
} else
{
return simIMU.getYaw();
@@ -748,9 +729,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getY())
: Rotation2d.fromRadians(imu.getRotation3d().getY());
return Rotation2d.fromRadians(imu.getRotation3d().getY());
} else
{
return simIMU.getPitch();
@@ -767,9 +746,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? Rotation2d.fromRadians(imu.getRotation3d().unaryMinus().getX())
: Rotation2d.fromRadians(imu.getRotation3d().getX());
return Rotation2d.fromRadians(imu.getRotation3d().getX());
} else
{
return simIMU.getRoll();
@@ -786,9 +763,7 @@ public class SwerveDrive
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!SwerveDriveTelemetry.isSimulation)
{
return swerveDriveConfiguration.invertedIMU
? imu.getRotation3d().unaryMinus()
: imu.getRotation3d();
return imu.getRotation3d();
} else
{
return simIMU.getGyroRotation3d();
@@ -953,7 +928,7 @@ public class SwerveDrive
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
@@ -969,7 +944,8 @@ public class SwerveDrive
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
module.updateTelemetry();
SmartDashboard.putNumber("Adjusted IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
@@ -1027,28 +1003,6 @@ public class SwerveDrive
}
}
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
* AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
* {@link SwerveDrive#setGyroOffset(Rotation3d)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
* {@link Timer#getFPGATimestamp()} or similar sources.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp)
{
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
robotPose.getRotation());
odometryLock.unlock();
setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
resetOdometry(newOdometry);
}
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement.
@@ -1066,8 +1020,31 @@ public class SwerveDrive
public void addVisionMeasurement(Pose2d robotPose, double timestamp,
Matrix<N3, N1> visionMeasurementStdDevs)
{
this.visionMeasurementStdDevs = visionMeasurementStdDevs;
addVisionMeasurement(robotPose, timestamp);
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs);
odometryLock.unlock();
}
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
* AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
* {@link SwerveDrive#setGyroOffset(Rotation3d)}.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
* {@link Timer#getFPGATimestamp()} or similar sources.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp)
{
odometryLock.lock();
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
// Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(),
// robotPose.getRotation());
odometryLock.unlock();
// setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians()));
// resetOdometry(newOdometry);
}

23
swervelib/imu/ADIS16448Swerve.java
View File

@@ -19,7 +19,11 @@ public class ADIS16448Swerve extends SwerveIMU
/**
* Offset for the ADIS16448.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -60,6 +64,16 @@ public class ADIS16448Swerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -67,9 +81,10 @@ public class ADIS16448Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
return new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
Math.toRadians(-imu.getGyroAngleY()),
Math.toRadians(-imu.getGyroAngleZ()));
Rotation3d reading = new Rotation3d(Math.toRadians(-imu.getGyroAngleX()),
Math.toRadians(-imu.getGyroAngleY()),
Math.toRadians(-imu.getGyroAngleZ()));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

19
swervelib/imu/ADIS16470Swerve.java
View File

@@ -20,7 +20,11 @@ public class ADIS16470Swerve extends SwerveIMU
/**
* Offset for the ADIS16470.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -62,6 +66,16 @@ public class ADIS16470Swerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -69,7 +83,8 @@ public class ADIS16470Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle(IMUAxis.kYaw)));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

19
swervelib/imu/ADXRS450Swerve.java
View File

@@ -19,7 +19,11 @@ public class ADXRS450Swerve extends SwerveIMU
/**
* Offset for the ADXRS450.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -60,6 +64,16 @@ public class ADXRS450Swerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -67,7 +81,8 @@ public class ADXRS450Swerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
return new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-imu.getAngle()));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

19
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -19,7 +19,11 @@ public class AnalogGyroSwerve extends SwerveIMU
/**
* Offset for the analog gyro.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
@@ -67,6 +71,16 @@ public class AnalogGyroSwerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -74,7 +88,8 @@ public class AnalogGyroSwerve extends SwerveIMU
*/
public Rotation3d getRawRotation3d()
{
return new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
Rotation3d reading = new Rotation3d(0, 0, Math.toRadians(-gyro.getAngle()));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

18
swervelib/imu/NavXSwerve.java
View File

@@ -23,7 +23,11 @@ public class NavXSwerve extends SwerveIMU
/**
* 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.
*/
@@ -124,6 +128,16 @@ public class NavXSwerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -132,7 +146,7 @@ public class NavXSwerve extends SwerveIMU
@Override
public Rotation3d getRawRotation3d()
{
return gyro.getRotation3d();
return invertedIMU ? gyro.getRotation3d().unaryMinus() : gyro.getRotation3d();
}
/**

33
swervelib/imu/Pigeon2Swerve.java
View File

@@ -23,7 +23,11 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Offset for the Pigeon 2.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for pigeon.
@@ -79,6 +83,16 @@ public class Pigeon2Swerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -88,14 +102,15 @@ public class Pigeon2Swerve extends SwerveIMU
public Rotation3d getRawRotation3d()
{
// TODO: Switch to suppliers.
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
return new Rotation3d(new Quaternion(w.refresh().getValue(),
x.refresh().getValue(),
y.refresh().getValue(),
z.refresh().getValue()));
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(),
x.refresh().getValue(),
y.refresh().getValue(),
z.refresh().getValue()));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

19
swervelib/imu/PigeonSwerve.java
View File

@@ -20,7 +20,11 @@ public class PigeonSwerve extends SwerveIMU
/**
* Offset for the Pigeon.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for pigeon.
@@ -62,6 +66,16 @@ public class PigeonSwerve extends SwerveIMU
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
@@ -72,7 +86,8 @@ public class PigeonSwerve extends SwerveIMU
{
double[] wxyz = new double[4];
imu.get6dQuaternion(wxyz);
return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**

7
swervelib/imu/SwerveIMU.java
View File

@@ -27,6 +27,13 @@ public abstract class SwerveIMU
*/
public abstract void setOffset(Rotation3d offset);
/**
* Set the gyro to invert its default direction.
*
* @param invertIMU gyro direction
*/
public abstract void setInverted(boolean invertIMU);
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*

8
swervelib/motors/TalonFXSwerve.java
View File

@@ -33,14 +33,14 @@ public class TalonFXSwerve extends SwerveMotor
* Velocity voltage setter for controlling drive motor.
*/
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
/**
* Conversion factor for the motor.
*/
private double conversionFactor;
/**
* TalonFX motor controller.
*/
TalonFX motor;
/**
* Conversion factor for the motor.
*/
private double conversionFactor;
/**
* Current TalonFX configuration.
*/

13
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -19,10 +19,6 @@ public class SwerveDriveConfiguration
* Swerve IMU
*/
public SwerveIMU imu;
/**
* Invert the imu measurements.
*/
public boolean invertedIMU = false;
/**
* Number of modules on the robot.
*/
@@ -54,7 +50,7 @@ public class SwerveDriveConfiguration
{
this.moduleCount = moduleConfigs.length;
this.imu = swerveIMU;
this.invertedIMU = invertedIMU;
swerveIMU.setInverted(invertedIMU);
this.modules = createModules(moduleConfigs, driveFeedforward);
this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
for (SwerveModule module : modules)
@@ -92,10 +88,11 @@ public class SwerveDriveConfiguration
Translation2d centerOfModules = moduleLocationsMeters[0];
//Calculate the Center by adding all module offsets together.
for(int i=1; i<moduleLocationsMeters.length; i++){
for (int i = 1; i < moduleLocationsMeters.length; i++)
{
centerOfModules = centerOfModules.plus(moduleLocationsMeters[i]);
}
}
//Return Largest Radius
return centerOfModules.getDistance(moduleLocationsMeters[0]);
}

3
swervelib/parser/json/DeviceJson.java
View File

@@ -124,6 +124,7 @@ public class DeviceJson
case "analog":
return new AnalogGyroSwerve(id);
case "navx":
case "navx_mxp_spi":
case "navx_spi":
return new NavXSwerve(SPI.Port.kMXP);
case "navx_i2c":
@@ -136,7 +137,7 @@ public class DeviceJson
return new NavXSwerve(I2C.Port.kMXP);
case "navx_usb":
return new NavXSwerve(Port.kUSB);
case "navx_mxp":
case "navx_mxp_serial":
return new NavXSwerve(Port.kMXP);
case "pigeon":
return new PigeonSwerve(id);

11
swervelib/telemetry/Alert.java
View File

@@ -49,23 +49,23 @@ public class Alert
/**
* Group of the alert.
*/
private static Map<String, SendableAlerts> groups = new HashMap<String, SendableAlerts>();
private static Map<String, SendableAlerts> groups = new HashMap<String, SendableAlerts>();
/**
* Type of the Alert to raise.
*/
private final AlertType type;
private final AlertType type;
/**
* Activation state of alert.
*/
private boolean active = false;
private boolean active = false;
/**
* When the alert was raised.
*/
private double activeStartTime = 0.0;
private double activeStartTime = 0.0;
/**
* Text of the alert.
*/
private String text;
private String text;
/**
* Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate
@@ -213,6 +213,7 @@ public class Alert
/**
* Get alerts based off of type.
*
* @param type Type of alert to fetch.
* @return Active alert strings.
*/