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 swervelib telemetry configurability. Reliable gyroscope modulo

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-24 19:11:05 -06:00
parent 9b699291e8
commit 69edd17103
121 changed files with 2280 additions and 501 deletions
Download Patch File Download Diff File Expand all files Collapse all files

258
swervelib/SwerveDrive.java
View File

@@ -1,5 +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;
@@ -10,9 +11,10 @@ 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.kinematics.SwerveModulePosition;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -26,6 +28,7 @@ import swervelib.parser.SwerveControllerConfiguration;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.simulation.SwerveIMUSimulation;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* Swerve Drive class representing and controlling the swerve drive.
@@ -52,11 +55,21 @@ public class SwerveDrive
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
public SwerveController swerveController;
/**
* Trustworthiness of the internal model of how motors should be moving Measured in expected standard deviation
* (meters of position and degrees of rotation)
*/
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
/**
* Trustworthiness of the vision system Measured in expected standard deviation (meters of position and degrees of
* rotation)
*/
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9, 0.9, 0.9);
/**
* Swerve IMU device for sensing the heading of the robot.
*/
@@ -95,7 +108,7 @@ public class SwerveDrive
// Create an integrator for angle if the robot is being simulated to emulate an IMU
// If the robot is real, instantiate the IMU instead.
if (RobotBase.isSimulation())
if (SwerveDriveTelemetry.isSimulation)
{
simIMU = new SwerveIMUSimulation();
} else
@@ -113,47 +126,50 @@ public class SwerveDrive
getYaw(),
getModulePositions(),
new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
VecBuilder.fill(
0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
VecBuilder.fill(
0.9, 0.9,
0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
stateStdDevs,
visionMeasurementStdDevs); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
// Initialize Telemetry
SmartDashboard.putData("Field", field);
SwerveDriveTelemetry.maxSpeed = swerveDriveConfiguration.maxSpeed;
SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
SwerveDriveTelemetry.moduleCount = swerveModules.length;
SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true,
false).moduleLocation.getX() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getX());
SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false,
true).moduleLocation.getY() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getY());
SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2];
for (SwerveModule module : swerveModules)
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches(
module.configuration.moduleLocation.getX());
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches(
module.configuration.moduleLocation.getY());
SmartDashboard.putData("Field", field);
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.maxSpeed = swerveDriveConfiguration.maxSpeed;
SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
SwerveDriveTelemetry.moduleCount = swerveModules.length;
SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true,
false).moduleLocation.getX() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getX());
SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false,
true).moduleLocation.getY() +
SwerveMath.getSwerveModule(swerveModules,
false,
false).moduleLocation.getY());
SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2];
for (SwerveModule module : swerveModules)
{
SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches(
module.configuration.moduleLocation.getX());
SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches(
module.configuration.moduleLocation.getY());
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
}
SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2];
// TODO: Might need to flip X and Y.
}
/**
* The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and
* commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel
* velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
* velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. This method
* defaults to no heading correction.
*
* @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
* second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
@@ -167,6 +183,28 @@ public class SwerveDrive
*/
public void drive(
Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
{
drive(translation, rotation, fieldRelative, isOpenLoop, false);
}
/**
* The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and
* commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel
* velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
*
* @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
* second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
* torwards port (left). In field-relative mode, positive x is away from the alliance wall
* (field North) and positive y is torwards the left wall when looking through the driver
* station glass (field West).
* @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot
* relativity.
* @param fieldRelative Drive mode. True for field-relative, false for robot-relative.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
* @param headingCorrection Whether to correct heading when driving translationally. Set to true to enable.
*/
public void drive(
Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, boolean headingCorrection)
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
@@ -178,7 +216,7 @@ public class SwerveDrive
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
if (Math.abs(rotation) < 0.01)
if (Math.abs(rotation) < 0.01 && headingCorrection)
{
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getYaw().getRadians());
@@ -188,10 +226,16 @@ public class SwerveDrive
}
// Display commanded speed for testing
SmartDashboard.putString("RobotVelocity", velocity.toString());
SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond);
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putString("RobotVelocity", velocity.toString());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond);
}
// Calculate required module states via kinematics
SwerveModuleState2[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity);
@@ -213,17 +257,23 @@ public class SwerveDrive
// Sets states
for (SwerveModule module : swerveModules)
{
SwerveModuleState2 moduleState = module.getState();
SwerveDriveTelemetry.desiredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop);
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Speed Setpoint: ",
desiredStates[module.moduleNumber].speedMetersPerSecond);
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Angle Setpoint: ",
desiredStates[module.moduleNumber].angle.getDegrees());
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveModuleState2 moduleState = module.getState();
SwerveDriveTelemetry.desiredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
}
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Speed Setpoint: ",
desiredStates[module.moduleNumber].speedMetersPerSecond);
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Angle Setpoint: ",
desiredStates[module.moduleNumber].angle.getDegrees());
}
}
}
@@ -295,7 +345,10 @@ public class SwerveDrive
*/
public void postTrajectory(Trajectory trajectory)
{
field.getObject("Trajectory").setTrajectory(trajectory);
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
field.getObject("Trajectory").setTrajectory(trajectory);
}
}
/**
@@ -336,7 +389,7 @@ public class SwerveDrive
{
// Resets the real gyro or the angle accumulator, depending on whether the robot is being
// simulated
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
imu.setYaw(0);
} else
@@ -355,7 +408,7 @@ public class SwerveDrive
public Rotation2d getYaw()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -374,7 +427,7 @@ public class SwerveDrive
public Rotation2d getPitch()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -393,7 +446,7 @@ public class SwerveDrive
public Rotation2d getRoll()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -412,7 +465,7 @@ public class SwerveDrive
public Rotation3d getGyroRotation3d()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -445,10 +498,20 @@ public class SwerveDrive
*/
public void lockPose()
{
// Sets states
for (SwerveModule swerveModule : swerveModules)
{
swerveModule.setDesiredState(
new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0), true);
SwerveModuleState2 desiredState =
new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0);
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
desiredState.angle.getDegrees();
SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] =
desiredState.speedMetersPerSecond;
}
swerveModule.setDesiredState(desiredState, false);
}
}
@@ -495,36 +558,47 @@ public class SwerveDrive
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
// Update angle accumulator if the robot is simulated
Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
if (RobotBase.isSimulation())
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
simIMU.updateOdometry(
kinematics,
getStates(),
modulePoses,
field);
Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
if (SwerveDriveTelemetry.isSimulation)
{
simIMU.updateOdometry(
kinematics,
getStates(),
modulePoses,
field);
}
ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
}
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
ChassisSpeeds measuredChassisSpeeds = getRobotVelocity();
SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond);
SwerveDriveTelemetry.robotRotation = getYaw().getDegrees();
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
}
double sumOmega = 0;
for (SwerveModule module : swerveModules)
{
SwerveModuleState2 moduleState = module.getState();
SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
sumOmega += Math.abs(moduleState.omegaRadPerSecond);
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
}
}
// If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
@@ -536,7 +610,10 @@ public class SwerveDrive
moduleSynchronizationCounter = 0;
}
SwerveDriveTelemetry.updateData();
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
SwerveDriveTelemetry.updateData();
}
}
/**
@@ -554,27 +631,30 @@ 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. <b>THIS WILL BREAK IF UPDATED TOO OFTEN.</b>
*
* @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.
* @param soft Add vision estimate using the
* {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)} function, or hard reset
* odometry with the given position with
* {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d,
* SwerveModulePosition[], Pose2d)}.
* @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.
* @param soft Add vision estimate using the
* {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)} function, or hard
* reset odometry with the given position with
* {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry#resetPosition(Rotation2d,
* SwerveModulePosition[], Pose2d)}.
* @param trustWorthiness Trust level of vision reading when using a soft measurement, used to multiply the standard
* deviation. Set to 1 for full trust.
*/
public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft)
public void addVisionMeasurement(Pose2d robotPose, double timestamp, boolean soft, double trustWorthiness)
{
if (soft)
{
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp,
visionMeasurementStdDevs.times(1.0 / trustWorthiness));
} else
{
swerveDrivePoseEstimator.resetPosition(
robotPose.getRotation(), getModulePositions(), robotPose);
}
if (!RobotBase.isSimulation())
if (!SwerveDriveTelemetry.isSimulation)
{
imu.setYaw(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getDegrees());
// Yaw reset recommended by Team 1622