+
edu.wpi.first.math.geometry.Pose2d
+
+
Gets the current pose (position and rotation) of the robot, as reported by odometry.
-
edu.wpi.first.math.kinematics.ChassisSpeeds
-
-
+
edu.wpi.first.math.kinematics.ChassisSpeeds
+
+
Gets the current robot-relative velocity (x, y and omega) of the robot
-
edu.wpi.first.math.geometry.Rotation2d
-
-
+
edu.wpi.first.math.geometry.Rotation2d
+
+
Gets the current roll angle of the robot, as reported by the imu.
-
-
-
+
+
+
Gets the current module states (azimuth and velocity)
-
-
-
+
+
+
Helper function to get the
swerveController for the
SwerveDrive which can be used to
generate
ChassisSpeeds for the robot to orient it correctly given axis or angles, and apply
SlewRateLimiter to given inputs.
-
edu.wpi.first.math.geometry.Pose2d[]
-
-
+
edu.wpi.first.math.geometry.Pose2d[]
+
+
Get the swerve module poses and on the field relative to the robot.
-
edu.wpi.first.math.geometry.Rotation2d
-
-
+
edu.wpi.first.math.geometry.Rotation2d
+
+
Gets the current yaw angle of the robot, as reported by the imu.
-
void
-
-
+
void
+
+
Point all modules toward the robot center, thus making the robot very difficult to move.
-
void
-
-
+
void
+
+
Post the trajectory to the field
-
void
-
-
+
void
+
+
Setup the swerve module feedforward.
-
void
-
-
+
void
+
+
Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
autonomous.
-
void
-
-
+
void
+
+
Resets odometry to the given pose.
-
void
-
-
+
void
+
+
Set chassis speeds with closed-loop velocity control.
-
void
-
setGyro (edu.wpi.first.math.geometry.Rotation3d gyro)
-
+
void
+
setGyro (edu.wpi.first.math.geometry.Rotation3d gyro)
+
Set the expected gyroscope angle using a Rotation3d object.
-
void
-
setModuleStates (SwerveModuleState2 [] desiredStates,
+void
+
-
+
Set the module states (azimuth and velocity) directly.
-
void
-
-
+
void
+
+
Sets the drive motors to brake/coast mode.
-
private void
-
setRawModuleStates (SwerveModuleState2 [] desiredStates,
+private void
+
-
+
Set the module states (azimuth and velocity) directly.
-
void
-
-
+
void
+
+
Synchronize angle motor integrated encoders with data from absolute encoders.
-
void
-
-
+
void
+
+
Update odometry should be run every loop.
-
void
-
-
+
void
+
+
Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
@@ -806,6 +815,29 @@ loadScripts(document, 'script');
+
+addVisionMeasurement
+public void addVisionMeasurement (edu.wpi.first.math.geometry.Pose2d robotPose,
+ double timestamp,
+ boolean soft,
+ edu.wpi.first.math.Matrix<edu.wpi.first.math.numbers.N3,edu.wpi.first.math.numbers.N1> visionMeasurementStdDevs)
+Add a vision measurement to the
SwerveDrivePoseEstimator and update the
SwerveIMU gyro reading with
+ the given timestamp of the vision measurement.
+Parameters:
+robotPose - Robot Pose2d as measured by vision.timestamp - Timestamp the measurement was taken as time since startup, should be taken from
+ Timer.getFPGATimestamp() or similar sources.soft - Add vision estimate using the
+ SwerveDrivePoseEstimator.addVisionMeasurement(Pose2d, double) function, or
+ hard reset odometry with the given position with
+ SwerveDriveOdometry.resetPosition(Rotation2d, SwerveModulePosition[], Pose2d).visionMeasurementStdDevs - Vision measurement standard deviation that will be sent to the
+ SwerveDrivePoseEstimator.
+
+
+
setGyro
public void setGyro (edu.wpi.first.math.geometry.Rotation3d gyro)
diff --git a/docs/swervelib/SwerveModule.html b/docs/swervelib/SwerveModule.html
index c1962c1..43e1356 100644
--- a/docs/swervelib/SwerveModule.html
+++ b/docs/swervelib/SwerveModule.html
@@ -1,11 +1,11 @@
-
+
SwerveModule
AnalogAbsoluteEncoderSwerve
CANCoderSwerve
SparkMaxEncoderSwerve
SwerveAbsoluteEncoder
swervelib.encoders
swervelib.encoders Class Hierarchy
ADIS16448Swerve
ADIS16470Swerve
ADXRS450Swerve
AnalogGyroSwerve
NavXSwerve
Pigeon2Swerve
PigeonSwerve
SwerveIMU
swervelib.imu
swervelib.imu Class Hierarchy
Matter
SwerveKinematics2
SwerveMath
SwerveModuleState2
swervelib.math
swervelib.math Class Hierarchy
SparkMaxBrushedMotorSwerve
SparkMaxSwerve.SparkMAX_slotIdx
SparkMaxSwerve
SwerveMotor
TalonFXSwerve
TalonSRXSwerve
swervelib.motors
swervelib.motors Class Hierarchy
swervelib
swervelib Class Hierarchy
PIDFConfig
SwerveControllerConfiguration
SwerveDriveConfiguration
SwerveModuleConfiguration
SwerveModulePhysicalCharacteristics
SwerveParser
PIDFRange
swervelib.parser.deserializer
swervelib.parser.deserializer Class Hierarchy
ControllerPropertiesJson
DeviceJson
ModuleJson
MotorConfigDouble
MotorConfigInt
PIDFPropertiesJson
PhysicalPropertiesJson
SwerveDriveJson
BoolMotorJson
LocationJson
swervelib.parser.json.modules
swervelib.parser.json.modules Class Hierarchy
swervelib.parser.json
swervelib.parser.json Class Hierarchy
swervelib.parser
swervelib.parser Class Hierarchy
SwerveIMUSimulation
SwerveModuleSimulation
PhysicsSim.SimProfile
PhysicsSim
TalonFXSimProfile
TalonSRXSimProfile
VictorSPXSimProfile
swervelib.simulation.ctre
swervelib.simulation.ctre Class Hierarchy
swervelib.simulation
swervelib.simulation Class Hierarchy
SwerveDriveTelemetry.TelemetryVerbosity
SwerveDriveTelemetry
swervelib.telemetry
swervelib.telemetry Class Hierarchy
visionMeasurementStdDevs)
+ {
+ this.visionMeasurementStdDevs = visionMeasurementStdDevs;
+ addVisionMeasurement(robotPose, timestamp, soft, 1);
+ }
+
+
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new
* {@link Rotation3d}.
diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java
index 57a462c..4745579 100644
--- a/swervelib/math/SwerveMath.java
+++ b/swervelib/math/SwerveMath.java
@@ -11,6 +11,8 @@ import swervelib.SwerveController;
import swervelib.SwerveModule;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.parser.SwerveModuleConfiguration;
+import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* Mathematical functions which pertain to swerve drive.
@@ -205,8 +207,10 @@ public class SwerveMath
double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
double maxAccel = 9.81 * horizontalDistance / robotCG.getZ();
-
- SmartDashboard.putNumber("calcMaxAccel", maxAccel);
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber("calcMaxAccel", maxAccel);
+ }
return maxAccel;
}
@@ -236,12 +240,18 @@ public class SwerveMath
{
// Get the robot's current field-relative velocity
Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
- SmartDashboard.putNumber("currentVelocity", currentVelocity.getX());
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber("currentVelocity", currentVelocity.getX());
+ }
// Calculate the commanded change in velocity by subtracting current velocity
// from commanded velocity
Translation2d deltaV = commandedVelocity.minus(currentVelocity);
- SmartDashboard.putNumber("deltaV", deltaV.getX());
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber("deltaV", deltaV.getX());
+ }
// Creates an acceleration vector with the direction of delta V and a magnitude
// of the maximum allowed acceleration in that direction