diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index d570293..ee9920d 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry
-
+
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index 4c48297..7f0d7d5 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.telemetry Class Hierarchy
-
+
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 7311e2b..a74806e 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -53,6 +53,26 @@ public class SwerveModule
* An {@link Alert} for if there is no Absolute Encoder on the module.
*/
private final Alert noEncoderWarning;
+ /**
+ * NT3 Raw Absolute Angle publisher for the absolute encoder.
+ */
+ private final String rawAbsoluteAngleName;
+ /**
+ * NT3 Adjusted Absolute angle publisher for the absolute encoder.
+ */
+ private final String adjAbsoluteAngleName;
+ /**
+ * NT3 Absolute encoder read issue.
+ */
+ private final String absoluteEncoderIssueName;
+ /**
+ * NT3 raw angle motor.
+ */
+ private final String rawAngleName;
+ /**
+ * NT3 Raw drive motor.
+ */
+ private final String rawDriveName;
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
@@ -176,6 +196,12 @@ public class SwerveModule
"Pushing the Absolute Encoder offset to the encoder failed on module #" +
moduleNumber,
Alert.AlertType.WARNING);
+
+ rawAbsoluteAngleName = "Module[" + configuration.name + "] Raw Absolute Encoder";
+ adjAbsoluteAngleName = "Module[" + configuration.name + "] Adjusted Absolute Encoder";
+ absoluteEncoderIssueName = "Module[" + configuration.name + "] Absolute Encoder Read Issue";
+ rawAngleName = "Module[" + configuration.name + "] Raw Angle Encoder";
+ rawDriveName = "Module[" + configuration.name + "] Raw Drive Encoder";
}
/**
@@ -293,7 +319,8 @@ public class SwerveModule
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
cosineScalar = Rotation2d.fromDegrees(desiredState.angle.getDegrees())
- .minus(Rotation2d.fromDegrees(getAbsolutePosition())).getCos(); // TODO: Investigate angle modulus by 180.
+ .minus(Rotation2d.fromDegrees(getAbsolutePosition()))
+ .getCos(); // TODO: Investigate angle modulus by 180.
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0)
{
@@ -526,13 +553,11 @@ public class SwerveModule
{
if (absoluteEncoder != null)
{
- SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder",
- absoluteEncoder.getAbsolutePosition());
+ SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition());
}
- SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Angle Encoder", angleMotor.getPosition());
- SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Drive Encoder", driveMotor.getPosition());
- SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition());
- SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue",
- getAbsoluteEncoderReadIssue() ? 1 : 0);
+ SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition());
+ SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition());
+ SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
+ SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
}
}
diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java
index dd81ada..6bd68da 100644
--- a/swervelib/encoders/SparkMaxEncoderSwerve.java
+++ b/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -35,6 +35,14 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
*/
public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor)
{
+ failureConfiguring = new Alert(
+ "Encoders",
+ "Failure configuring SparkMax Analog Encoder",
+ Alert.AlertType.WARNING_TRACE);
+ offsetFailure = new Alert(
+ "Encoders",
+ "Failure to set Absolute Encoder Offset",
+ Alert.AlertType.WARNING_TRACE);
if (motor.getMotor() instanceof CANSparkMax)
{
encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
@@ -44,14 +52,6 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
}
- failureConfiguring = new Alert(
- "Encoders",
- "Failure configuring SparkMax Analog Encoder",
- Alert.AlertType.WARNING_TRACE);
- offsetFailure = new Alert(
- "Encoders",
- "Failure to set Absolute Encoder Offset",
- Alert.AlertType.WARNING_TRACE);
}
/**
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
index 4db3ea1..bc89264 100644
--- a/swervelib/imu/Pigeon2Swerve.java
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -1,11 +1,9 @@
package swervelib.imu;
-import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
-import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -20,7 +18,7 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Wait time for status frames to show up.
*/
- public static double STATUS_TIMEOUT_SECONDS = 0.04;
+ public static double STATUS_TIMEOUT_SECONDS = 0.04;
/**
* Pigeon2 IMU device.
*/
@@ -28,11 +26,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;
+ private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for pigeon.
@@ -106,31 +104,7 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Rotation3d getRawRotation3d()
{
- // TODO: Switch to suppliers.
- StatusSignal w = imu.getQuatW();
- StatusSignal x = imu.getQuatX();
- StatusSignal y = imu.getQuatY();
- StatusSignal z = imu.getQuatZ();
- if(w.getStatus() != StatusCode.OK)
- {
- w = w.waitForUpdate(STATUS_TIMEOUT_SECONDS);
- }
- if(x.getStatus() != StatusCode.OK)
- {
- x = x.waitForUpdate(STATUS_TIMEOUT_SECONDS);
- }
- if(y.getStatus() != StatusCode.OK)
- {
- y = y.waitForUpdate(STATUS_TIMEOUT_SECONDS);
- }
- if(z.getStatus() != StatusCode.OK)
- {
- z = z.waitForUpdate(STATUS_TIMEOUT_SECONDS);
- }
- Rotation3d reading = new Rotation3d(new Quaternion(w.getValue(),
- x.getValue(),
- y.getValue(),
- z.getValue()));
+ Rotation3d reading = imu.getRotation3d();
return invertedIMU ? reading.unaryMinus() : reading;
}
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index e7018f0..bb33fc4 100644
--- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -68,6 +68,17 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution,
boolean useDataPortQuadEncoder)
{
+
+ noEncoderAlert = new Alert("Motors",
+ "Cannot use motor without encoder.",
+ Alert.AlertType.ERROR_TRACE);
+ failureConfiguringAlert = new Alert("Motors",
+ "Failure configuring motor " + motor.getDeviceId(),
+ Alert.AlertType.WARNING_TRACE);
+ noEncoderDefinedAlert = new Alert("Motors",
+ "An encoder MUST be defined to work with a SparkMAX",
+ Alert.AlertType.ERROR_TRACE);
+
// Drive motors **MUST** have an encoder attached.
if (isDriveMotor && encoderType == Type.kNoSensor)
{
@@ -103,15 +114,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback.
- noEncoderAlert = new Alert("Motors",
- "Cannot use motor without encoder.",
- Alert.AlertType.ERROR_TRACE);
- failureConfiguringAlert = new Alert("Motors",
- "Failure configuring motor " + motor.getDeviceId(),
- Alert.AlertType.WARNING_TRACE);
- noEncoderDefinedAlert = new Alert("Motors",
- "An encoder MUST be defined to work with a SparkMAX",
- Alert.AlertType.ERROR_TRACE);
}
/**
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index b2d2eec..213f199 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -17,6 +17,10 @@ import swervelib.telemetry.SwerveDriveTelemetry;
public class TalonFXSwerve extends SwerveMotor
{
+ /**
+ * Wait time for status frames to show up.
+ */
+ public static double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Factory default already occurred.
*/
@@ -33,10 +37,6 @@ public class TalonFXSwerve extends SwerveMotor
* Velocity voltage setter for controlling drive motor.
*/
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
- /**
- * Wait time for status frames to show up.
- */
- public static double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* TalonFX motor controller.
*/
@@ -44,11 +44,11 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Conversion factor for the motor.
*/
- private double conversionFactor;
+ private double conversionFactor;
/**
* Current TalonFX configuration.
*/
- private TalonFXConfiguration configuration = new TalonFXConfiguration();
+ private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java
index 2bdadfb..5239c44 100644
--- a/swervelib/parser/json/DeviceJson.java
+++ b/swervelib/parser/json/DeviceJson.java
@@ -1,5 +1,9 @@
package swervelib.parser.json;
+import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning;
+import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning;
+import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
+
import com.revrobotics.SparkRelativeEncoder.Type;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.I2C;
@@ -26,8 +30,6 @@ import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSwerve;
import swervelib.motors.TalonSRXSwerve;
-import swervelib.telemetry.Alert;
-import swervelib.telemetry.Alert.AlertType;
/**
* Device JSON parsed class. Used to access the JSON data.
@@ -35,36 +37,18 @@ import swervelib.telemetry.Alert.AlertType;
public class DeviceJson
{
- /**
- * An {@link Alert} for if the CAN ID is greater than 40.
- */
- private final Alert canIdWarning = new Alert("JSON",
- "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
- Alert.AlertType.WARNING);
- /**
- * An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
- */
- private final Alert i2cLockupWarning = new Alert("IMU",
- "I2C lockup issue detected on roboRIO. Check console for more information.",
- Alert.AlertType.WARNING);
- /**
- * NavX serial comm issue.
- */
- private final Alert serialCommsIssueWarning = new Alert("IMU",
- "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
- AlertType.WARNING);
/**
* The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx
*/
- public String type;
+ public String type;
/**
* The CAN ID or pin ID of the device.
*/
- public int id;
+ public int id;
/**
* The CAN bus name which the device resides on if using CAN.
*/
- public String canbus = "";
+ public String canbus = "";
/**
* Create a {@link SwerveAbsoluteEncoder} from the current configuration.
diff --git a/swervelib/telemetry/Alert.java b/swervelib/telemetry/Alert.java
index f3c6af3..40f9af7 100644
--- a/swervelib/telemetry/Alert.java
+++ b/swervelib/telemetry/Alert.java
@@ -34,11 +34,9 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
-import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
-import java.util.function.Predicate;
/**
* Class for managing persistent alerts to be sent over NetworkTables.
@@ -111,24 +109,7 @@ public class Alert
if (active && !this.active)
{
activeStartTime = Timer.getFPGATimestamp();
- switch (type)
- {
- case ERROR:
- DriverStation.reportError(text, false);
- break;
- case ERROR_TRACE:
- DriverStation.reportError(text, true);
- break;
- case WARNING:
- DriverStation.reportWarning(text, false);
- break;
- case WARNING_TRACE:
- DriverStation.reportWarning(text, true);
- break;
- case INFO:
- System.out.println(text);
- break;
- }
+ printAlert(text);
}
this.active = active;
}
@@ -142,28 +123,39 @@ public class Alert
{
if (active && !text.equals(this.text))
{
- switch (type)
- {
- case ERROR:
- DriverStation.reportError(text, false);
- break;
- case ERROR_TRACE:
- DriverStation.reportError(text, true);
- break;
- case WARNING:
- DriverStation.reportWarning(text, false);
- break;
- case WARNING_TRACE:
- DriverStation.reportWarning(text, true);
- break;
- case INFO:
- System.out.println(text);
- break;
- }
+ printAlert(text);
}
this.text = text;
}
+
+ /**
+ * Print the alert message.
+ *
+ * @param text Text to print.
+ */
+ private void printAlert(String text)
+ {
+ switch (type)
+ {
+ case ERROR:
+ DriverStation.reportError(text, false);
+ break;
+ case ERROR_TRACE:
+ DriverStation.reportError(text, true);
+ break;
+ case WARNING:
+ DriverStation.reportWarning(text, false);
+ break;
+ case WARNING_TRACE:
+ DriverStation.reportWarning(text, true);
+ break;
+ case INFO:
+ System.out.println(text);
+ break;
+ }
+ }
+
/**
* Represents an alert's level of urgency.
*/
@@ -219,14 +211,16 @@ public class Alert
*/
public String[] getStrings(AlertType type)
{
- Predicate activeFilter = (Alert x) -> x.type == type && x.active;
- Comparator timeSorter =
- (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
- return alerts.stream()
- .filter(activeFilter)
- .sorted(timeSorter)
- .map((Alert a) -> a.text)
- .toArray(String[]::new);
+ List alertStrings = new ArrayList<>();
+ for (Alert alert : alerts)
+ {
+ if (alert.type == type && alert.active)
+ {
+ alertStrings.add(alert.text);
+ }
+ }
+ // alertStrings.sort((a1, a2) -> (int) (a2.activeStartTime - a1.activeStartTime));
+ return alertStrings.toArray(new String[alertStrings.size()]);
}
@Override
diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java
index 7c1f546..8a300d9 100644
--- a/swervelib/telemetry/SwerveDriveTelemetry.java
+++ b/swervelib/telemetry/SwerveDriveTelemetry.java
@@ -2,6 +2,7 @@ package swervelib.telemetry;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import swervelib.telemetry.Alert.AlertType;
/**
* Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit)
@@ -9,69 +10,87 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveDriveTelemetry
{
+ /**
+ * An {@link Alert} for if the CAN ID is greater than 40.
+ */
+ public static final Alert canIdWarning = new Alert("JSON",
+ "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
+ Alert.AlertType.WARNING);
+ /**
+ * An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
+ */
+ public static final Alert i2cLockupWarning = new Alert("IMU",
+ "I2C lockup issue detected on roboRIO. Check console for more information.",
+ Alert.AlertType.WARNING);
+ /**
+ * NavX serial comm issue.
+ */
+ public static final Alert serialCommsIssueWarning = new Alert("IMU",
+ "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
+ AlertType.WARNING);
/**
* The current telemetry verbosity level.
*/
- public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
+ public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
/**
* State of simulation of the Robot, used to optimize retrieval.
*/
- public static boolean isSimulation = RobotBase.isSimulation();
+ public static boolean isSimulation = RobotBase.isSimulation();
/**
* The number of swerve modules
*/
- public static int moduleCount;
+ public static int moduleCount;
/**
* The number of swerve modules
*/
- public static double[] wheelLocations;
+ 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