+
static double
+
+
The distance between the left and right modules.
+
+
+
+
The current telemetry verbosity level.
+
static double[]
@@ -206,6 +233,20 @@ loadScripts(document, 'script');
Field Details
+
+verbosity
+
+The current telemetry verbosity level.
+
+
+
+
+isSimulation
+public static boolean isSimulation
+State of simulation of the Robot, used to optimize retrieval.
+
+
+
moduleCount
public static int moduleCount
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index a49fad8..87e3dc6 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry
JavaScript is disabled on your browser.
@@ -80,14 +84,20 @@ loadScripts(document, 'script');
-
Classes
-
+
All Classes and Interfaces Classes Enum Classes
+
+
Telemetry to describe the
SwerveDrive following frc-web-components.
+
+
+
Verbosity of telemetry data sent back.
+
+
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index f5466d1..9c8c9be 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry Class Hierarchy
-
+
@@ -64,6 +64,20 @@ loadScripts(document, 'script');
+
+Enum Class Hierarchy
+
+
diff --git a/docs/type-search-index.js b/docs/type-search-index.js
index e90a937..5dea219 100644
--- a/docs/type-search-index.js
+++ b/docs/type-search-index.js
@@ -1 +1 @@
-typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveKinematics2"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.math","l":"SwerveModuleState2"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
+typeSearchIndex = [{"p":"swervelib.imu","l":"ADIS16448Swerve"},{"p":"swervelib.imu","l":"ADIS16470Swerve"},{"p":"swervelib.imu","l":"ADXRS450Swerve"},{"l":"All Classes and Interfaces","u":"allclasses-index.html"},{"p":"swervelib.encoders","l":"AnalogAbsoluteEncoderSwerve"},{"p":"swervelib.imu","l":"AnalogGyroSwerve"},{"p":"swervelib.parser.json.modules","l":"BoolMotorJson"},{"p":"swervelib.encoders","l":"CANCoderSwerve"},{"p":"swervelib.parser.json","l":"ControllerPropertiesJson"},{"p":"swervelib.parser.json","l":"DeviceJson"},{"p":"swervelib.parser.json.modules","l":"LocationJson"},{"p":"swervelib.parser.json","l":"ModuleJson"},{"p":"swervelib.parser.json","l":"MotorConfigDouble"},{"p":"swervelib.parser.json","l":"MotorConfigInt"},{"p":"swervelib.imu","l":"NavXSwerve"},{"p":"swervelib.parser.json","l":"PhysicalPropertiesJson"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim"},{"p":"swervelib.parser","l":"PIDFConfig"},{"p":"swervelib.parser.json","l":"PIDFPropertiesJson"},{"p":"swervelib.parser.deserializer","l":"PIDFRange"},{"p":"swervelib.imu","l":"Pigeon2Swerve"},{"p":"swervelib.imu","l":"PigeonSwerve"},{"p":"swervelib.simulation.ctre","l":"PhysicsSim.SimProfile"},{"p":"swervelib.motors","l":"SparkMaxSwerve.SparkMAX_slotIdx"},{"p":"swervelib.motors","l":"SparkMaxBrushedMotorSwerve"},{"p":"swervelib.encoders","l":"SparkMaxEncoderSwerve"},{"p":"swervelib.motors","l":"SparkMaxSwerve"},{"p":"swervelib.encoders","l":"SwerveAbsoluteEncoder"},{"p":"swervelib","l":"SwerveController"},{"p":"swervelib.parser","l":"SwerveControllerConfiguration"},{"p":"swervelib","l":"SwerveDrive"},{"p":"swervelib.parser","l":"SwerveDriveConfiguration"},{"p":"swervelib.parser.json","l":"SwerveDriveJson"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry"},{"p":"swervelib.imu","l":"SwerveIMU"},{"p":"swervelib.simulation","l":"SwerveIMUSimulation"},{"p":"swervelib.math","l":"SwerveKinematics2"},{"p":"swervelib.math","l":"SwerveMath"},{"p":"swervelib","l":"SwerveModule"},{"p":"swervelib.parser","l":"SwerveModuleConfiguration"},{"p":"swervelib.parser","l":"SwerveModulePhysicalCharacteristics"},{"p":"swervelib.simulation","l":"SwerveModuleSimulation"},{"p":"swervelib.math","l":"SwerveModuleState2"},{"p":"swervelib.motors","l":"SwerveMotor"},{"p":"swervelib.parser","l":"SwerveParser"},{"p":"swervelib.simulation.ctre","l":"TalonFXSimProfile"},{"p":"swervelib.motors","l":"TalonFXSwerve"},{"p":"swervelib.simulation.ctre","l":"TalonSRXSimProfile"},{"p":"swervelib.motors","l":"TalonSRXSwerve"},{"p":"swervelib.telemetry","l":"SwerveDriveTelemetry.TelemetryVerbosity"},{"p":"swervelib.simulation.ctre","l":"VictorSPXSimProfile"}];updateSearchResults();
\ No newline at end of file
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 837768a..bd2949a 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -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
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 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. THIS WILL BREAK IF UPDATED TOO OFTEN.
*
- * @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
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index e6722ef..d97c5f2 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -4,13 +4,14 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveModuleState2;
import swervelib.motors.SwerveMotor;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
+import swervelib.telemetry.SwerveDriveTelemetry;
+import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
@@ -110,7 +111,7 @@ public class SwerveModule
driveMotor.burnFlash();
angleMotor.burnFlash();
- if (RobotBase.isSimulation())
+ if (SwerveDriveTelemetry.isSimulation)
{
simModule = new SwerveModuleSimulation();
}
@@ -143,13 +144,15 @@ public class SwerveModule
desiredState =
new SwerveModuleState2(
simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond);
-
- SmartDashboard.putNumber(
- "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
- SmartDashboard.putNumber(
- "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
- SmartDashboard.putNumber(
- "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber(
+ "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
+ SmartDashboard.putNumber(
+ "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
+ SmartDashboard.putNumber(
+ "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
+ }
if (isOpenLoop)
{
@@ -170,7 +173,7 @@ public class SwerveModule
angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
lastAngle = angle;
- if (RobotBase.isSimulation())
+ if (SwerveDriveTelemetry.isSimulation)
{
simModule.updateStateAndPosition(desiredState);
}
@@ -197,7 +200,7 @@ public class SwerveModule
double velocity;
Rotation2d azimuth;
double omega;
- if (!RobotBase.isSimulation())
+ if (!SwerveDriveTelemetry.isSimulation)
{
velocity = driveMotor.getVelocity();
azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -218,7 +221,7 @@ public class SwerveModule
{
double position;
Rotation2d azimuth;
- if (!RobotBase.isSimulation())
+ if (!SwerveDriveTelemetry.isSimulation)
{
position = driveMotor.getPosition();
azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -226,7 +229,10 @@ public class SwerveModule
{
return simModule.getPosition();
}
- SmartDashboard.putNumber("Module " + moduleNumber + "Angle", azimuth.getDegrees());
+ if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+ {
+ SmartDashboard.putNumber("Module " + moduleNumber + "Angle", azimuth.getDegrees());
+ }
return new SwerveModulePosition(position, azimuth);
}
diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java
index 6313ea8..1aa483d 100644
--- a/swervelib/imu/ADIS16448Swerve.java
+++ b/swervelib/imu/ADIS16448Swerve.java
@@ -34,7 +34,7 @@ public class ADIS16448Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- yawOffset = imu.getAngle() % 360;
+ yawOffset = Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -54,7 +54,7 @@ public class ADIS16448Swerve extends SwerveIMU
@Override
public void setYaw(double yaw)
{
- yawOffset = (yaw % 360) + (imu.getAngle() % 360);
+ yawOffset = Math.IEEEremainder(yaw, 360) + Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -65,9 +65,9 @@ public class ADIS16448Swerve extends SwerveIMU
@Override
public void getYawPitchRoll(double[] yprArray)
{
- yprArray[0] = (imu.getAngle() % 360) - yawOffset;
- yprArray[1] = imu.getXComplementaryAngle() % 360;
- yprArray[2] = imu.getYComplementaryAngle() % 360;
+ yprArray[0] = Math.IEEEremainder(imu.getAngle(), 360) - yawOffset;
+ yprArray[1] = Math.IEEEremainder(imu.getXComplementaryAngle(), 360);
+ yprArray[2] = Math.IEEEremainder(imu.getYComplementaryAngle(), 360);
}
/**
diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java
index 34e8aef..d0f01bb 100644
--- a/swervelib/imu/ADIS16470Swerve.java
+++ b/swervelib/imu/ADIS16470Swerve.java
@@ -34,7 +34,7 @@ public class ADIS16470Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- yawOffset = imu.getAngle() % 360;
+ yawOffset = Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -54,7 +54,7 @@ public class ADIS16470Swerve extends SwerveIMU
@Override
public void setYaw(double yaw)
{
- yawOffset = (yaw % 360) + (imu.getAngle() % 360);
+ yawOffset = Math.IEEEremainder(yaw, 360) + Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -65,9 +65,9 @@ public class ADIS16470Swerve extends SwerveIMU
@Override
public void getYawPitchRoll(double[] yprArray)
{
- yprArray[0] = (imu.getAngle() % 360) - yawOffset;
- yprArray[1] = imu.getXComplementaryAngle() % 360;
- yprArray[2] = imu.getYComplementaryAngle() % 360;
+ yprArray[0] = Math.IEEEremainder(imu.getAngle(), 360) - yawOffset;
+ yprArray[1] = Math.IEEEremainder(imu.getXComplementaryAngle(), 360);
+ yprArray[2] = Math.IEEEremainder(imu.getYComplementaryAngle(), 360);
}
/**
diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java
index 9ce3cb9..6a9d97f 100644
--- a/swervelib/imu/ADXRS450Swerve.java
+++ b/swervelib/imu/ADXRS450Swerve.java
@@ -34,7 +34,7 @@ public class ADXRS450Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
- yawOffset = imu.getAngle() % 360;
+ yawOffset = Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -54,7 +54,7 @@ public class ADXRS450Swerve extends SwerveIMU
@Override
public void setYaw(double yaw)
{
- yawOffset = (yaw % 360) + (imu.getAngle() % 360);
+ yawOffset = Math.IEEEremainder(yaw, 360) + Math.IEEEremainder(imu.getAngle(), 360);
}
/**
@@ -65,7 +65,7 @@ public class ADXRS450Swerve extends SwerveIMU
@Override
public void getYawPitchRoll(double[] yprArray)
{
- yprArray[0] = (imu.getAngle() % 360) - yawOffset;
+ yprArray[0] = Math.IEEEremainder(imu.getAngle(), 360) - yawOffset;
yprArray[1] = 0;
yprArray[2] = 0;
}
diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java
index 15a63d4..1cd60cd 100644
--- a/swervelib/imu/AnalogGyroSwerve.java
+++ b/swervelib/imu/AnalogGyroSwerve.java
@@ -41,7 +41,7 @@ public class AnalogGyroSwerve extends SwerveIMU
@Override
public void factoryDefault()
{
- yawOffset = gyro.getAngle() % 360;
+ yawOffset = Math.IEEEremainder(gyro.getAngle(), 360);
}
/**
@@ -61,7 +61,7 @@ public class AnalogGyroSwerve extends SwerveIMU
@Override
public void setYaw(double yaw)
{
- yawOffset = (yaw % 360) + (gyro.getAngle() % 360);
+ yawOffset = Math.IEEEremainder(yaw, 360) + Math.IEEEremainder(gyro.getAngle(), 360);
}
/**
@@ -72,7 +72,7 @@ public class AnalogGyroSwerve extends SwerveIMU
@Override
public void getYawPitchRoll(double[] yprArray)
{
- yprArray[0] = (gyro.getAngle() % 360) - yawOffset;
+ yprArray[0] = Math.IEEEremainder(gyro.getAngle(), 360) - yawOffset;
yprArray[1] = 0;
yprArray[2] = 0;
}
diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java
index f52c25a..46aa41c 100644
--- a/swervelib/imu/NavXSwerve.java
+++ b/swervelib/imu/NavXSwerve.java
@@ -47,7 +47,7 @@ public class NavXSwerve extends SwerveIMU
public void factoryDefault()
{
// gyro.reset(); // Reported to be slow
- yawOffset = gyro.getYaw() % 360;
+ yawOffset = Math.IEEEremainder(gyro.getYaw(), 360);
}
/**
@@ -67,7 +67,7 @@ public class NavXSwerve extends SwerveIMU
public void setYaw(double yaw)
{
// gyro.reset(); // Reported to be slow using the offset.
- yawOffset = (yaw % 360) + (gyro.getYaw() % 360);
+ yawOffset = Math.IEEEremainder(yaw, 360) + Math.IEEEremainder(gyro.getYaw(), 360);
}
/**
@@ -78,9 +78,10 @@ public class NavXSwerve extends SwerveIMU
@Override
public void getYawPitchRoll(double[] yprArray)
{
- yprArray[0] = (gyro.getYaw() % 360) - yawOffset;
- yprArray[1] = gyro.getPitch() % 360;
- yprArray[2] = gyro.getRoll() % 360;
+
+ yprArray[0] = (Math.IEEEremainder(gyro.getYaw(), 360)) - yawOffset;
+ yprArray[1] = Math.IEEEremainder(gyro.getPitch(), 360);
+ yprArray[2] = Math.IEEEremainder(gyro.getRoll(), 360);
}
/**
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
new file mode 100644
index 0000000..a5d1a2a
--- /dev/null
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -0,0 +1,382 @@
+package swervelib.motors;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxAlternateEncoder;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkMaxRelativeEncoder.Type;
+import edu.wpi.first.wpilibj.DriverStation;
+import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.motors.SparkMaxSwerve.SparkMAX_slotIdx;
+import swervelib.parser.PIDFConfig;
+
+/**
+ * Brushed motor control with SparkMax.
+ */
+public class SparkMaxBrushedMotorSwerve extends SwerveMotor
+{
+
+ /**
+ * SparkMAX Instance.
+ */
+ public CANSparkMax motor;
+
+ /**
+ * Absolute encoder attached to the SparkMax (if exists)
+ */
+ public AbsoluteEncoder absoluteEncoder;
+ /**
+ * Integrated encoder.
+ */
+ public RelativeEncoder encoder;
+ /**
+ * Closed-loop PID controller.
+ */
+ public SparkMaxPIDController pid;
+ /**
+ * Factory default already occurred.
+ */
+ private boolean factoryDefaultOccurred = false;
+
+ /**
+ * Initialize the swerve motor.
+ *
+ * @param motor The SwerveMotor as a SparkMax object.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
+ * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
+ * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
+ */
+ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
+ boolean useDataPortQuadEncoder)
+ {
+ // Drive motors **MUST** have an encoder attached.
+ if (isDriveMotor && encoderType == Type.kNoSensor)
+ {
+ DriverStation.reportError("Cannot use motor without encoder.", true);
+ throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached.");
+ }
+
+ // Hall encoders can be used as quadrature encoders.
+ if (encoderType == Type.kHallSensor)
+ {
+ encoderType = Type.kQuadrature;
+ }
+
+ this.motor = motor;
+ this.isDriveMotor = isDriveMotor;
+
+ factoryDefaults();
+ clearStickyFaults();
+
+ // Get the onboard PID controller.
+ pid = motor.getPIDController();
+
+ // If there is a sensor attached to the data port or encoder port set the relative encoder.
+ if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder))
+ {
+ this.encoder = useDataPortQuadEncoder ?
+ motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) :
+ motor.getEncoder(encoderType, countsPerRevolution);
+
+ // Configure feedback of the PID controller as the integrated encoder.
+ pid.setFeedbackDevice(encoder);
+ }
+
+ motor.setCANTimeout(0); // Spin off configurations in a different thread.
+ }
+
+ /**
+ * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor.
+ *
+ * @param id CAN ID of the SparkMax.
+ * @param isDriveMotor Is the motor being initialized a drive motor?
+ * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device.
+ * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution.
+ * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder.
+ */
+ public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
+ boolean useDataPortQuadEncoder)
+ {
+ this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution,
+ useDataPortQuadEncoder);
+ }
+
+ /**
+ * Set the voltage compensation for the swerve module motor.
+ *
+ * @param nominalVoltage Nominal voltage for operation to output to.
+ */
+ @Override
+ public void setVoltageCompensation(double nominalVoltage)
+ {
+ motor.enableVoltageCompensation(nominalVoltage);
+ }
+
+ /**
+ * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+ * voltage compensation. This is useful to protect the motor from current spikes.
+ *
+ * @param currentLimit Current limit in AMPS at free speed.
+ */
+ @Override
+ public void setCurrentLimit(int currentLimit)
+ {
+ motor.setSmartCurrentLimit(currentLimit);
+ }
+
+ /**
+ * Set the maximum rate the open/closed loop output can change by.
+ *
+ * @param rampRate Time in seconds to go from 0 to full throttle.
+ */
+ @Override
+ public void setLoopRampRate(double rampRate)
+ {
+ motor.setOpenLoopRampRate(rampRate);
+ motor.setClosedLoopRampRate(rampRate);
+ }
+
+ /**
+ * Get the motor object from the module.
+ *
+ * @return Motor object.
+ */
+ @Override
+ public Object getMotor()
+ {
+ return motor;
+ }
+
+ /**
+ * Queries whether the absolute encoder is directly attached to the motor controller.
+ *
+ * @return connected absolute encoder state.
+ */
+ @Override
+ public boolean isAttachedAbsoluteEncoder()
+ {
+ return absoluteEncoder != null;
+ }
+
+ /**
+ * Configure the factory defaults.
+ */
+ @Override
+ public void factoryDefaults()
+ {
+ if (!factoryDefaultOccurred)
+ {
+ motor.restoreFactoryDefaults();
+ factoryDefaultOccurred = true;
+ }
+ }
+
+ /**
+ * Clear the sticky faults on the motor controller.
+ */
+ @Override
+ public void clearStickyFaults()
+ {
+ motor.clearFaults();
+ }
+
+ /**
+ * Set the absolute encoder to be a compatible absolute encoder.
+ *
+ * @param encoder The encoder to use.
+ * @return The {@link SwerveMotor} for easy instantiation.
+ */
+ @Override
+ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
+ {
+ if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
+ {
+ absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
+ pid.setFeedbackDevice(absoluteEncoder);
+ }
+ if (absoluteEncoder == null && this.encoder == null)
+ {
+ DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true);
+ throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX");
+ }
+ return this;
+ }
+
+ /**
+ * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
+ *
+ * @param positionConversionFactor The conversion factor to apply.
+ */
+ @Override
+ public void configureIntegratedEncoder(double positionConversionFactor)
+ {
+ if (absoluteEncoder == null)
+ {
+ encoder.setPositionConversionFactor(positionConversionFactor);
+ encoder.setVelocityConversionFactor(positionConversionFactor / 60);
+
+ // Taken from
+ // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+ configureCANStatusFrames(10, 20, 20, 500, 500);
+ } else
+ {
+ absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
+ absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ }
+ }
+
+ /**
+ * Configure the PIDF values for the closed loop controller.
+ *
+ * @param config Configuration class holding the PIDF values.
+ */
+ @Override
+ public void configurePIDF(PIDFConfig config)
+ {
+ int pidSlot =
+ isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pid.setP(config.p, pidSlot);
+ pid.setI(config.i, pidSlot);
+ pid.setD(config.d, pidSlot);
+ pid.setFF(config.f, pidSlot);
+ pid.setIZone(config.iz, pidSlot);
+ pid.setOutputRange(config.output.min, config.output.max, pidSlot);
+ }
+
+ /**
+ * Configure the PID wrapping for the position closed loop controller.
+ *
+ * @param minInput Minimum PID input.
+ * @param maxInput Maximum PID input.
+ */
+ @Override
+ public void configurePIDWrapping(double minInput, double maxInput)
+ {
+ pid.setPositionPIDWrappingEnabled(true);
+ pid.setPositionPIDWrappingMinInput(minInput);
+ pid.setPositionPIDWrappingMaxInput(maxInput);
+ }
+
+ /**
+ * Set the CAN status frames.
+ *
+ * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower
+ * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current
+ * @param CANStatus2 Motor Position
+ * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
+ * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
+ */
+ public void configureCANStatusFrames(
+ int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
+ {
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3);
+ motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4);
+ // TODO: Configure Status Frame 5 and 6 if necessary
+ // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
+ }
+
+ /**
+ * Set the idle mode.
+ *
+ * @param isBrakeMode Set the brake mode.
+ */
+ @Override
+ public void setMotorBrake(boolean isBrakeMode)
+ {
+ motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ }
+
+ /**
+ * Set the motor to be inverted.
+ *
+ * @param inverted State of inversion.
+ */
+ @Override
+ public void setInverted(boolean inverted)
+ {
+ motor.setInverted(inverted);
+ }
+
+ /**
+ * Save the configurations from flash to EEPROM.
+ */
+ @Override
+ public void burnFlash()
+ {
+ motor.burnFlash();
+ }
+
+ /**
+ * Set the percentage output.
+ *
+ * @param percentOutput percent out for the motor controller.
+ */
+ @Override
+ public void set(double percentOutput)
+ {
+ motor.set(percentOutput);
+ }
+
+ /**
+ * Set the closed loop PID controller reference point.
+ *
+ * @param setpoint Setpoint in MPS or Angle in degrees.
+ * @param feedforward Feedforward in volt-meter-per-second or kV.
+ */
+ @Override
+ public void setReference(double setpoint, double feedforward)
+ {
+ int pidSlot =
+ isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ pid.setReference(
+ setpoint,
+ isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
+ pidSlot,
+ feedforward);
+ }
+
+ /**
+ * Get the velocity of the integrated encoder.
+ *
+ * @return velocity
+ */
+ @Override
+ public double getVelocity()
+ {
+ return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ }
+
+ /**
+ * Get the position of the integrated encoder.
+ *
+ * @return Position
+ */
+ @Override
+ public double getPosition()
+ {
+ return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition();
+ }
+
+ /**
+ * Set the integrated encoder position.
+ *
+ * @param position Integrated encoder position.
+ */
+ @Override
+ public void setPosition(double position)
+ {
+ if (absoluteEncoder == null)
+ {
+ encoder.setPosition(position);
+ }
+ }
+}
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index 8e231ff..21c975e 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -6,10 +6,10 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import edu.wpi.first.wpilibj.RobotBase;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.simulation.ctre.PhysicsSim;
+import swervelib.telemetry.SwerveDriveTelemetry;
/**
* {@link com.ctre.phoenix.motorcontrol.can.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics.
@@ -60,7 +60,7 @@ public class TalonFXSwerve extends SwerveMotor
factoryDefaults();
clearStickyFaults();
- if (RobotBase.isSimulation())
+ if (SwerveDriveTelemetry.isSimulation)
{
PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
}
@@ -247,7 +247,7 @@ public class TalonFXSwerve extends SwerveMotor
{
double lowerBound;
double upperBound;
- double lowerOffset = scopeReference % 360;
+ double lowerOffset = Math.IEEEremainder(scopeReference, 360);
// Create the interval from the reference angle.
if (lowerOffset >= 0)
@@ -301,7 +301,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
- if (RobotBase.isSimulation())
+ if (SwerveDriveTelemetry.isSimulation)
{
PhysicsSim.getInstance().run();
}
@@ -346,7 +346,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
- if (!absoluteEncoder && !RobotBase.isSimulation())
+ if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
motor.setSelectedSensorPosition(position / positionConversionFactor, 0, 250);
}
diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java
index 59a5056..013e3f8 100644
--- a/swervelib/motors/TalonSRXSwerve.java
+++ b/swervelib/motors/TalonSRXSwerve.java
@@ -6,9 +6,9 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import edu.wpi.first.wpilibj.RobotBase;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
+import swervelib.telemetry.SwerveDriveTelemetry;
/**
* {@link com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX} Swerve Motor.
@@ -234,7 +234,7 @@ public class TalonSRXSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
- if (!absoluteEncoder && !RobotBase.isSimulation())
+ if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
motor.setSelectedSensorPosition(convertToNativeSensorUnits(position));
}
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
index 9923a6b..52d6ba0 100644
--- a/swervelib/parser/json/DeviceJson.java
+++ b/swervelib/parser/json/DeviceJson.java
@@ -1,5 +1,6 @@
package swervelib.parser.json;
+import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.SerialPort.Port;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
@@ -13,6 +14,7 @@ import swervelib.imu.NavXSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.PigeonSwerve;
import swervelib.imu.SwerveIMU;
+import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSwerve;
@@ -105,6 +107,30 @@ public class DeviceJson
{
switch (type)
{
+ case "sparkmax_brushed":
+ switch (canbus)
+ {
+ case "greyhill_63r256":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, false);
+ case "srx_mag_encoder":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, false);
+ case "throughbore":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 8192, false);
+ case "throughbore_dataport":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 8192, true);
+ case "greyhill_63r256_dataport":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 1024, true);
+ case "srx_mag_encoder_dataport":
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kQuadrature, 4096, true);
+ default:
+ if (isDriveMotor)
+ {
+ throw new RuntimeException("Spark MAX " + id + " MUST have a encoder attached to the motor controller.");
+ }
+ // We are creating a motor for an angle motor which will use the absolute encoder attached to the data port.
+ return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false);
+ }
+ case "neo":
case "sparkmax":
return new SparkMaxSwerve(id, isDriveMotor);
case "falcon":
diff --git a/swervelib/simulation/ctre/PhysicsSim.java b/swervelib/simulation/ctre/PhysicsSim.java
index 1b9122d..189e49c 100644
--- a/swervelib/simulation/ctre/PhysicsSim.java
+++ b/swervelib/simulation/ctre/PhysicsSim.java
@@ -11,8 +11,8 @@ import java.util.ArrayList;
public class PhysicsSim
{
- private static final PhysicsSim sim = new PhysicsSim();
- private final ArrayList _simProfiles = new ArrayList();
+ private static PhysicsSim sim;
+ private final ArrayList _simProfiles = new ArrayList();
/**
* Gets the robot simulator instance.
@@ -21,6 +21,10 @@ public class PhysicsSim
*/
public static PhysicsSim getInstance()
{
+ if (sim == null)
+ {
+ sim = new PhysicsSim();
+ }
return sim;
}
diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java
index c5ed414..7c1f546 100644
--- a/swervelib/telemetry/SwerveDriveTelemetry.java
+++ b/swervelib/telemetry/SwerveDriveTelemetry.java
@@ -1,5 +1,6 @@
package swervelib.telemetry;
+import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
@@ -9,60 +10,68 @@ public class SwerveDriveTelemetry
{
/**
- * The number of swerve modules
+ * The current telemetry verbosity level.
*/
- public static int moduleCount;
+ public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
+ /**
+ * State of simulation of the Robot, used to optimize retrieval.
+ */
+ public static boolean isSimulation = RobotBase.isSimulation();
/**
* The number of swerve modules
*/
- public static double[] wheelLocations;
+ public static int moduleCount;
+ /**
+ * The number of swerve modules
+ */
+ public static double[] wheelLocations;
/**
* An array of rotation and velocity values describing the measured state of each swerve module
*/
- public static double[] measuredStates;
+ public static double[] measuredStates;
/**
* An array of rotation and velocity values describing the desired state of each swerve module
*/
- public static double[] desiredStates;
+ public static double[] desiredStates;
/**
* The robot's current rotation based on odometry or gyro readings
*/
- public static double robotRotation = 0;
+ public static double robotRotation = 0;
/**
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
*/
- public static double maxSpeed;
+ public static double maxSpeed;
/**
* The units of the module rotations and robot rotation
*/
- public static String rotationUnit = "degrees";
+ public static String rotationUnit = "degrees";
/**
* The distance between the left and right modules.
*/
- public static double sizeLeftRight;
+ public static double sizeLeftRight;
/**
* The distance between the front and back modules.
*/
- public static double sizeFrontBack;
+ public static double sizeFrontBack;
/**
* The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to
* align with odometry data or match videos. 'up', 'right', 'down' or 'left'
*/
- public static String forwardDirection = "up";
+ public static String forwardDirection = "up";
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
- public static double maxAngularVelocity;
+ public static double maxAngularVelocity;
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
- public static double[] measuredChassisSpeeds = new double[3];
+ public static double[] measuredChassisSpeeds = new double[3];
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
- public static double[] desiredChassisSpeeds = new double[3];
+ public static double[] desiredChassisSpeeds = new double[3];
/**
* Upload data to smartdashboard
@@ -83,4 +92,27 @@ public class SwerveDriveTelemetry
SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds);
SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds);
}
+
+ /**
+ * Verbosity of telemetry data sent back.
+ */
+ public enum TelemetryVerbosity
+ {
+ /**
+ * No telemetry data is sent back.
+ */
+ NONE,
+ /**
+ * Low telemetry data, only post the robot position on the field.
+ */
+ LOW,
+ /**
+ * Full swerve drive data is sent back in both human and machine readable forms.
+ */
+ HIGH,
+ /**
+ * Only send the machine readable data related to swerve drive.
+ */
+ MACHINE
+ }
}