From dba98491e81060055fbdc998c3985d98534f56c3 Mon Sep 17 00:00:00 2001 From: gaming <48131223+TheGamer1002@users.noreply.github.com> Date: Tue, 16 Jan 2024 16:54:53 -0500 Subject: [PATCH 01/15] Implement NetworkAlerts --- .../drivebase/AbsoluteDriveAdv.java | 1 - src/main/java/swervelib/SwerveDrive.java | 1 - src/main/java/swervelib/SwerveModule.java | 21 +- .../encoders/AnalogAbsoluteEncoderSwerve.java | 17 +- .../swervelib/encoders/CANCoderSwerve.java | 49 +++-- .../encoders/PWMDutyCycleEncoderSwerve.java | 10 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 18 +- .../encoders/SparkMaxEncoderSwerve.java | 17 +- src/main/java/swervelib/imu/NavXSwerve.java | 15 +- src/main/java/swervelib/math/SwerveMath.java | 2 - .../swervelib/motors/SparkFlexSwerve.java | 17 +- .../motors/SparkMaxBrushedMotorSwerve.java | 24 ++- .../swervelib/parser/json/DeviceJson.java | 19 +- src/main/java/swervelib/telemetry/Alert.java | 195 ++++++++++++++++++ 14 files changed, 342 insertions(+), 64 deletions(-) create mode 100644 src/main/java/swervelib/telemetry/Alert.java diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 1c725458..7a97907c 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0915b08d..b13f7202 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 82b3ca92..5827c99f 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -5,13 +5,13 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.motors.SwerveMotor; import swervelib.parser.SwerveModuleConfiguration; import swervelib.simulation.SwerveModuleSimulation; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -61,6 +61,14 @@ public class SwerveModule * Encoder synchronization queued. */ private boolean synchronizeEncoderQueued = false; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private Alert encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING); + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private Alert noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, Alert.AlertType.WARNING); /** * Construct the swerve module and initialize the swerve module motors and absolute encoder. @@ -397,14 +405,11 @@ public void pushOffsetsToControllers() if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { angleOffset = 0; - } else - { - DriverStation.reportWarning( - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); + } else { + encoderOffsetWarning.set(true); } - } else - { - DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + } else { + noEncoderWarning.set(true); } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 1b830b1b..33cfd76d 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -1,8 +1,8 @@ package swervelib.encoders; import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. @@ -19,6 +19,16 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state of the encoder. */ private boolean inverted = false; + /** An {@link Alert} for if the absolute encoder offset cannot be set. */ + private Alert cannotSetOffset = new Alert( + "Encoders", + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + Alert.AlertType.WARNING); + /** An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */ + private Alert inaccurateVelocities = new Alert( + "Encoders", + "The Analog Absolute encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); /** * Construct the Thrifty Encoder as a Swerve Absolute Encoder. @@ -101,8 +111,7 @@ public Object getAbsoluteEncoder() public boolean setAbsoluteEncoderOffset(double offset) { //Do Nothing - DriverStation.reportWarning( - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); + cannotSetOffset.set(true); return false; } @@ -114,7 +123,7 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.getValue(); } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 5d8c3283..2871ff95 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.wpilibj.DriverStation; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for CTRE CANCoders. @@ -21,6 +21,28 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder * CANCoder with WPILib sendable and support. */ public CANcoder encoder; + /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ + private Alert magnetFieldLessThanIdeal = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the CANCoder reading is faulty. */ + private Alert readingFaulty = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ + private Alert readingIgnored = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the absolute encoder offset cannot be set. */ + private Alert cannotSetOffset = new Alert( + "Encoders", + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset", + Alert.AlertType.WARNING); /** * Initialize the CANCoder on the standard CANBus. @@ -91,15 +113,14 @@ public double getAbsolutePosition() if (strength != MagnetHealthValue.Magnet_Green) { - DriverStation.reportWarning( - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); - } + magnetFieldLessThanIdeal.set(true); + } else magnetFieldLessThanIdeal.set(false); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + readingFaulty.set(true); return 0; - } + }else readingFaulty.set(false); StatusSignal angle = encoder.getAbsolutePosition().refresh(); // Taken from democat's library. @@ -115,8 +136,8 @@ public double getAbsolutePosition() if (angle.getStatus() != StatusCode.OK) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); - } + readingIgnored.set(true); + } else readingIgnored.set(false); return angle.getValue() * 360; } @@ -149,12 +170,16 @@ public boolean setAbsoluteEncoderOffset(double offset) return false; } error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); - if (error == StatusCode.OK) - { + cannotSetOffset.setText( + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); + if (error == StatusCode.OK) { + cannotSetOffset.set(false); return true; } - DriverStation.reportWarning( - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); + cannotSetOffset.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 0361aec8..4f83aa3e 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -1,7 +1,7 @@ package swervelib.encoders; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import swervelib.telemetry.Alert; /** * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag @@ -22,6 +22,12 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state. */ private boolean isInverted; + /** An {@link Alert} for if the encoder cannot report accurate velocities. */ + private Alert inaccurateVelocities = new Alert( + "Encoders", + "The PWM Duty Cycle encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); + /** * Constructor for the PWM duty cycle encoder. @@ -74,7 +80,7 @@ public Object getAbsoluteEncoder() @Override public double getVelocity() { - DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.get(); } diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 0508bca8..b8b04004 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,9 +4,10 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor.Mode; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port analog pin. @@ -18,6 +19,17 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ public SparkAnalogSensor encoder; + /** An {@link Alert} for if there is a failure configuring the encoder. */ + private Alert failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the absolute encoder does not support integrated offsets. */ + private Alert doesNotSupportIntegratedOffsets = new Alert( + "Encoders", + "SparkMax Analog Sensors do not support integrated offsets", + Alert.AlertType.WARNING_TRACE); + /** * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data @@ -50,7 +62,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -113,7 +125,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); + doesNotSupportIntegratedOffsets.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index f95d36d2..da773d9c 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,9 +4,10 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.SparkAbsoluteEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port. @@ -18,6 +19,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ public AbsoluteEncoder encoder; + /** An {@link Alert} for if there is a failure configuring the encoder. */ + private Alert failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + private Alert offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + Alert.AlertType.WARNING_TRACE); /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. @@ -52,7 +62,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -124,7 +134,8 @@ public boolean setAbsoluteEncoderOffset(double offset) return true; } } - DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); + offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error); + offsetFailure.set(true); return false; } diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 272efafa..621fd2d2 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -1,14 +1,14 @@ package swervelib.imu; import com.kauailabs.navx.frc.AHRS; -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.DriverStation; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.telemetry.Alert; + import java.util.Optional; /** @@ -25,6 +25,8 @@ public class NavXSwerve extends SwerveIMU * Offset for the NavX. */ private Rotation3d offset = new Rotation3d(); + /** An {@link Alert} for if there is an error instantiating the NavX. */ + private Alert navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); /** * Constructor for the NavX swerve. @@ -43,7 +45,8 @@ public NavXSwerve(SerialPort.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -64,7 +67,8 @@ public NavXSwerve(SPI.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -85,7 +89,8 @@ public NavXSwerve(I2C.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index 0b4fdd5e..5de06d59 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -13,8 +13,6 @@ import swervelib.SwerveModule; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveModuleConfiguration; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** * Mathematical functions which pertain to swerve drive. diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a5aef968..a8b7e159 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -12,10 +12,11 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -44,6 +45,13 @@ public class SparkFlexSwerve extends SwerveMotor * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** An {@link Alert} for if there is an error configuring the motor. */ + private Alert failureConfiguring = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ + private Alert absoluteEncoderOffsetWarning = new Alert("Motors", + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + + "absoluteEncoderOffset in the Swerve Module JSON!", + Alert.AlertType.WARNING); /** * Initialize the swerve motor. @@ -92,7 +100,7 @@ private void configureSparkFlex(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguring.set(true); } /** @@ -185,10 +193,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); + absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fc7d94ed..53604739 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,20 +1,16 @@ package swervelib.motors; -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; +import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; + +import java.util.function.Supplier; /** * Brushed motor control with SparkMax. @@ -43,6 +39,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** An {@link Alert} for if the motor has no encoder. */ + private Alert noEncoderAlert = new Alert("Motors", "Cannot use motor without encoder.", Alert.AlertType.ERROR_TRACE); + /** An {@link Alert} for if there is an error configuring the motor. */ + private Alert failureConfiguringAlert = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the motor has no encoder defined. */ + private Alert noEncoderDefinedAlert = new Alert("Motors","An encoder MUST be defined to work with a SparkMAX", Alert.AlertType.ERROR_TRACE); /** * Initialize the swerve motor. @@ -59,7 +61,7 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type // Drive motors **MUST** have an encoder attached. if (isDriveMotor && encoderType == Type.kNoSensor) { - DriverStation.reportError("Cannot use motor without encoder.", true); + noEncoderAlert.set(true); throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached."); } @@ -122,7 +124,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguringAlert.set(true); } /** @@ -220,7 +222,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) } if (absoluteEncoder == null && this.encoder == null) { - DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); + noEncoderDefinedAlert.set(true); throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); } return this; diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 9b3a8aac..6ee795a5 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -26,6 +26,7 @@ import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; +import swervelib.telemetry.Alert; /** * Device JSON parsed class. Used to access the JSON data. @@ -45,6 +46,14 @@ public class DeviceJson * The CAN bus name which the device resides on if using CAN. */ public String canbus = ""; + /** + * An {@link Alert} for if the CAN ID is greater than 40. + */ + private 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 Alert i2cLockupWarning = new Alert("IMU", "I2C lockup issue detected on roboRIO. Check console for more information.", Alert.AlertType.WARNING); /** * Create a {@link SwerveAbsoluteEncoder} from the current configuration. @@ -57,8 +66,7 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -99,8 +107,7 @@ public SwerveIMU createIMU() { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -121,6 +128,7 @@ public SwerveIMU createIMU() "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + ".html#onboard-i2c-causing-system-lockups", false); + i2cLockupWarning.set(true); return new NavXSwerve(I2C.Port.kMXP); case "navx_usb": return new NavXSwerve(Port.kUSB); @@ -145,8 +153,7 @@ public SwerveMotor createMotor(boolean isDriveMotor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java new file mode 100644 index 00000000..56f0c5da --- /dev/null +++ b/src/main/java/swervelib/telemetry/Alert.java @@ -0,0 +1,195 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found below. + +// MIT License +// +// Copyright (c) 2023 FRC 6328 +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +package swervelib.telemetry; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +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. */ +public class Alert { + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + 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; + } + } + this.active = active; + } + + /** Updates current alert text. */ + public void setText(String text) { + 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; + } + } + this.text = text; + } + + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); + + 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); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } + + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + * Trace printed to driver station console. + */ + ERROR_TRACE, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + * Trace printed to driver station console. + */ + WARNING_TRACE, + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } +} \ No newline at end of file From 53cfd409b5c9b8d4e2dce4f5b3f7d58111d09db8 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Tue, 16 Jan 2024 20:27:11 -0600 Subject: [PATCH 02/15] Fixed Rotation being off in AbsoluteDriveAdv The rotation was off by 90 degrees and I also changed how it controls slightly. --- src/main/java/frc/robot/Constants.java | 2 +- .../drivebase/AbsoluteDriveAdv.java | 50 +++++++++---------- 2 files changed, 24 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3be8a30f..96f74374 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,6 @@ public static class OperatorConstants public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; public static final double RIGHT_X_DEADBAND = 0.01; - public static final double TURN_CONSTANT = 0.75; + public static final double TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 235ab5d7..061eed66 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -27,7 +27,7 @@ public class AbsoluteDriveAdv extends CommandBase private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingAdjust; - private boolean initRotation = false; + private boolean resetHeading = false; private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; /** @@ -67,7 +67,7 @@ public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplie @Override public void initialize() { - initRotation = true; + resetHeading = true; } // Called every time the scheduler runs while the command is scheduled. @@ -76,53 +76,43 @@ public void execute() { double headingX = 0; double headingY = 0; - Rotation2d newHeading = Rotation2d.fromRadians(0); // These are written to allow combinations for 45 angles // Face Away from Drivers if(lookAway.getAsBoolean()){ - headingX = 1; + headingY = -1; } // Face Right if(lookRight.getAsBoolean()){ - headingY = 1; + headingX = 1; } // Face Left if(lookLeft.getAsBoolean()){ - headingY = -1; + headingX = -1; } // Face Towards the Drivers if(lookTowards.getAsBoolean()){ - headingX = -1; - } - - //Dont overwrite a button press - if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){ - newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()) - .plus(swerve.getHeading()); - headingX = newHeading.getSin(); - headingY = newHeading.getCos(); + headingY = 1; } - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), - headingX, - headingY); - // Prevent Movement After Auto - if(initRotation) + if(resetHeading) { - if(headingX == 0 && headingY == 0) + if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { - // Get the curretHeading - Rotation2d firstLoopHeading = swerve.getHeading(); + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); } - //Dont Init Rotation Again - initRotation = false; + //Dont reset Heading Again + resetHeading = false; } + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + // Limit velocity to prevent tippy Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), @@ -132,7 +122,13 @@ public void execute() SmartDashboard.putString("Translation", translation.toString()); // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){ + resetHeading = true; + swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); + } + else{ + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } } // Called once the command ends or is interrupted. From a63d2384b731b5ca1b625fc717727910dee40d02 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:21:18 -0600 Subject: [PATCH 03/15] Updated formatting. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/SwerveDrive.java | 22 +- src/main/java/swervelib/SwerveModule.java | 16 +- .../encoders/AnalogAbsoluteEncoderSwerve.java | 14 +- .../swervelib/encoders/CANCoderSwerve.java | 51 +-- .../swervelib/encoders/CanAndCoderSwerve.java | 2 +- .../encoders/PWMDutyCycleEncoderSwerve.java | 6 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 15 +- .../encoders/SparkMaxEncoderSwerve.java | 11 +- .../encoders/SwerveAbsoluteEncoder.java | 1 + .../java/swervelib/imu/ADIS16470Swerve.java | 2 +- src/main/java/swervelib/imu/NavXSwerve.java | 9 +- .../java/swervelib/imu/Pigeon2Swerve.java | 17 +- .../swervelib/motors/SparkFlexSwerve.java | 56 ++-- .../motors/SparkMaxBrushedMotorSwerve.java | 40 ++- .../java/swervelib/motors/TalonFXSwerve.java | 16 +- .../parser/SwerveDriveConfiguration.java | 15 +- .../swervelib/parser/json/DeviceJson.java | 14 +- src/main/java/swervelib/telemetry/Alert.java | 294 ++++++++++-------- 18 files changed, 341 insertions(+), 260 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index b13f7202..aeef17b6 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -66,6 +66,10 @@ public class SwerveDrive * Odometry lock to ensure thread safety. */ private final Lock odometryLock = new ReentrantLock(); + /** + * Deadband for speeds in heading correction. + */ + private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * Field object. */ @@ -99,14 +103,14 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; - /** - * Whether heading correction PID is currently active. - */ - private boolean correctionEnabled = false; /** * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Whether heading correction PID is currently active. + */ + private boolean correctionEnabled = false; /** * Swerve IMU device for sensing the heading of the robot. */ @@ -119,10 +123,6 @@ public class SwerveDrive * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ private int moduleSynchronizationCounter = 0; - /** - * Deadband for speeds in heading correction. - */ - private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * The last heading set in radians. */ @@ -412,10 +412,10 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent if (headingCorrection) { if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND - && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) + && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND + || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) { - if (!correctionEnabled) + if (!correctionEnabled) { lastHeadingRadians = getYaw().getRadians(); correctionEnabled = true; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 5827c99f..61d2b590 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -64,11 +64,17 @@ public class SwerveModule /** * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. */ - private Alert encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING); + private Alert encoderOffsetWarning = new Alert("Motors", + "Pushing the Absolute Encoder offset to the encoder failed on module #" + + moduleNumber, + Alert.AlertType.WARNING); /** * An {@link Alert} for if there is no Absolute Encoder on the module. */ - private Alert noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, Alert.AlertType.WARNING); + private Alert noEncoderWarning = new Alert("Motors", + "There is no Absolute Encoder on module #" + + moduleNumber, + Alert.AlertType.WARNING); /** * Construct the swerve module and initialize the swerve module motors and absolute encoder. @@ -405,10 +411,12 @@ public void pushOffsetsToControllers() if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { angleOffset = 0; - } else { + } else + { encoderOffsetWarning.set(true); } - } else { + } else + { noEncoderWarning.set(true); } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 33cfd76d..f0af1a03 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -18,14 +18,18 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder /** * Inversion state of the encoder. */ - private boolean inverted = false; - /** An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset = new Alert( + private boolean inverted = false; + /** + * An {@link Alert} for if the absolute encoder offset cannot be set. + */ + private Alert cannotSetOffset = new Alert( "Encoders", "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), Alert.AlertType.WARNING); - /** An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */ - private Alert inaccurateVelocities = new Alert( + /** + * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. + */ + private Alert inaccurateVelocities = new Alert( "Encoders", "The Analog Absolute encoder may not report accurate velocities!", Alert.AlertType.WARNING_TRACE); diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 22f97bd4..7a71875e 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -20,28 +20,36 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder /** * CANCoder with WPILib sendable and support. */ - public CANcoder encoder; - /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ - private Alert magnetFieldLessThanIdeal = new Alert( + public CANcoder encoder; + /** + * An {@link Alert} for if the CANCoder magnet field is less than ideal. + */ + private Alert magnetFieldLessThanIdeal = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", Alert.AlertType.WARNING); - /** An {@link Alert} for if the CANCoder reading is faulty. */ - private Alert readingFaulty = new Alert( + /** + * An {@link Alert} for if the CANCoder reading is faulty. + */ + private Alert readingFaulty = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " reading was faulty.", Alert.AlertType.WARNING); - /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ - private Alert readingIgnored = new Alert( + /** + * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. + */ + private Alert readingIgnored = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", Alert.AlertType.WARNING); - /** An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset = new Alert( + /** + * An {@link Alert} for if the absolute encoder offset cannot be set. + */ + private Alert cannotSetOffset = new Alert( "Encoders", "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset", + + encoder.getDeviceID() + + " Absolute Encoder Offset", Alert.AlertType.WARNING); /** @@ -111,16 +119,13 @@ public double getAbsolutePosition() readingError = false; MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - if (strength != MagnetHealthValue.Magnet_Green) - { - magnetFieldLessThanIdeal.set(true); - } else magnetFieldLessThanIdeal.set(false); + magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; readingFaulty.set(true); return 0; - }else + } else { readingFaulty.set(false); } @@ -141,7 +146,10 @@ public double getAbsolutePosition() { readingError = true; readingIgnored.set(true); - } else readingIgnored.set(false); + } else + { + readingIgnored.set(false); + } return angle.getValue() * 360; } @@ -176,10 +184,11 @@ public boolean setAbsoluteEncoderOffset(double offset) error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); cannotSetOffset.setText( "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset Error: " - + error); - if (error == StatusCode.OK) { + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); + if (error == StatusCode.OK) + { cannotSetOffset.set(false); return true; } diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 9ab00987..1c6b7b13 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -25,7 +25,7 @@ public CanAndCoderSwerve(int canid) /** * Reset the encoder to factory defaults. - * + *

* This will not clear the stored zero offset. */ @Override diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 4f83aa3e..d43c80ed 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -22,8 +22,10 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state. */ private boolean isInverted; - /** An {@link Alert} for if the encoder cannot report accurate velocities. */ - private Alert inaccurateVelocities = new Alert( + /** + * An {@link Alert} for if the encoder cannot report accurate velocities. + */ + private Alert inaccurateVelocities = new Alert( "Encoders", "The PWM Duty Cycle encoder may not report accurate velocities!", Alert.AlertType.WARNING_TRACE); diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index b8b04004..22084d32 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,7 +4,6 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor.Mode; - import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -18,14 +17,18 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ - public SparkAnalogSensor encoder; - /** An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring = new Alert( + public SparkAnalogSensor encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring = new Alert( "Encoders", "Failure configuring SparkMax Analog Encoder", Alert.AlertType.WARNING_TRACE); - /** An {@link Alert} for if the absolute encoder does not support integrated offsets. */ - private Alert doesNotSupportIntegratedOffsets = new Alert( + /** + * An {@link Alert} for if the absolute encoder does not support integrated offsets. + */ + private Alert doesNotSupportIntegratedOffsets = new Alert( "Encoders", "SparkMax Analog Sensors do not support integrated offsets", Alert.AlertType.WARNING_TRACE); diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index da773d9c..4d8ed8e2 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,7 +4,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.SparkAbsoluteEncoder.Type; - import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -18,13 +17,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ - public AbsoluteEncoder encoder; - /** An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring = new Alert( + public AbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring = new Alert( "Encoders", "Failure configuring SparkMax Analog Encoder", Alert.AlertType.WARNING_TRACE); - private Alert offsetFailure = new Alert( + private Alert offsetFailure = new Alert( "Encoders", "Failure to set Absolute Encoder Offset", Alert.AlertType.WARNING_TRACE); diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index a01e4c47..c992b81a 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -56,6 +56,7 @@ public abstract class SwerveAbsoluteEncoder /** * Get the velocity in degrees/sec. + * * @return velocity in degrees/sec. */ public abstract double getVelocity(); diff --git a/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index 5a102bf0..562bbd1e 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -2,8 +2,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 621fd2d2..8fa09fdb 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -7,9 +7,8 @@ import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import swervelib.telemetry.Alert; - import java.util.Optional; +import swervelib.telemetry.Alert; /** * Communicates with the NavX as the IMU. @@ -24,8 +23,10 @@ public class NavXSwerve extends SwerveIMU /** * Offset for the NavX. */ - private Rotation3d offset = new Rotation3d(); - /** An {@link Alert} for if there is an error instantiating the NavX. */ + private Rotation3d offset = new Rotation3d(); + /** + * An {@link Alert} for if there is an error instantiating the NavX. + */ private Alert navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); /** diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 61fb6f83..25a41ea2 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -88,11 +88,14 @@ public void setOffset(Rotation3d offset) public Rotation3d getRawRotation3d() { // TODO: Switch to suppliers. - StatusSignal w = imu.getQuatW(); - StatusSignal x = imu.getQuatX(); - StatusSignal y = imu.getQuatY(); - StatusSignal z = imu.getQuatZ(); - return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())); + StatusSignal w = imu.getQuatW(); + StatusSignal x = imu.getQuatX(); + StatusSignal y = imu.getQuatY(); + StatusSignal z = imu.getQuatZ(); + return new Rotation3d(new Quaternion(w.refresh().getValue(), + x.refresh().getValue(), + y.refresh().getValue(), + z.refresh().getValue())); } /** @@ -120,7 +123,9 @@ public Optional getAccel() StatusSignal yAcc = imu.getAccelerationX(); StatusSignal zAcc = imu.getAccelerationX(); - return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0)); + return Optional.of(new Translation3d(xAcc.refresh().getValue(), + yAcc.refresh().getValue(), + zAcc.refresh().getValue()).times(9.81 / 16384.0)); } /** diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a8b7e159..22216fe4 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,18 +1,17 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; - import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -28,11 +27,11 @@ public class SparkFlexSwerve extends SwerveMotor /** * SparkMAX Instance. */ - public CANSparkFlex motor; + public CANSparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ @@ -40,18 +39,25 @@ public class SparkFlexSwerve extends SwerveMotor /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; - /** An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguring = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); - /** An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ - private Alert absoluteEncoderOffsetWarning = new Alert("Motors", - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + - "absoluteEncoderOffset in the Swerve Module JSON!", - Alert.AlertType.WARNING); + private boolean factoryDefaultOccurred = false; + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + private Alert failureConfiguring = new Alert("Motors", + "Failure configuring motor " + + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + /** + * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. + */ + private Alert absoluteEncoderOffsetWarning = new Alert("Motors", + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + + "absoluteEncoderOffset in the Swerve Module JSON!", + Alert.AlertType.WARNING); /** * Initialize the swerve motor. @@ -362,20 +368,20 @@ public void setReference(double setpoint, double feedforward) if (isDriveMotor) { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); } else { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); - if(SwerveDriveTelemetry.isSimulation) + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + if (SwerveDriveTelemetry.isSimulation) { encoder.setPosition(setpoint); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 53604739..6d461594 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,17 +1,21 @@ package swervelib.motors; -import com.revrobotics.*; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAlternateEncoder; +import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; +import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; -import java.util.function.Supplier; - /** * Brushed motor control with SparkMax. */ @@ -26,11 +30,11 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Absolute encoder attached to the SparkMax (if exists) */ - public AbsoluteEncoder absoluteEncoder; + public AbsoluteEncoder absoluteEncoder; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Closed-loop PID controller. */ @@ -38,13 +42,25 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; - /** An {@link Alert} for if the motor has no encoder. */ - private Alert noEncoderAlert = new Alert("Motors", "Cannot use motor without encoder.", Alert.AlertType.ERROR_TRACE); - /** An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguringAlert = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); - /** An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert = new Alert("Motors","An encoder MUST be defined to work with a SparkMAX", Alert.AlertType.ERROR_TRACE); + private boolean factoryDefaultOccurred = false; + /** + * An {@link Alert} for if the motor has no encoder. + */ + private Alert noEncoderAlert = new Alert("Motors", + "Cannot use motor without encoder.", + Alert.AlertType.ERROR_TRACE); + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + private Alert failureConfiguringAlert = new Alert("Motors", + "Failure configuring motor " + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + /** + * An {@link Alert} for if the motor has no encoder defined. + */ + private Alert noEncoderDefinedAlert = new Alert("Motors", + "An encoder MUST be defined to work with a SparkMAX", + Alert.AlertType.ERROR_TRACE); /** * Initialize the swerve motor. diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index af29e21a..2deefb35 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -21,10 +21,6 @@ public class TalonFXSwerve extends SwerveMotor * Factory default already occurred. */ private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Whether the absolute encoder is integrated. */ @@ -33,18 +29,22 @@ public class TalonFXSwerve extends SwerveMotor * Motion magic angle voltage setter. */ private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); -// /** -// * Motion Magic exponential voltage setters. -// */ -// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); /** * Velocity voltage setter for controlling drive motor. */ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); +// /** +// * Motion Magic exponential voltage setters. +// */ +// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); /** * TalonFX motor controller. */ TalonFX motor; + /** + * Current TalonFX configuration. + */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index 3e416783..06ec164d 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -91,16 +91,19 @@ public double getDriveBaseRadiusMeters() { //Find Center of Robot by adding all module offsets together. Should be zero, but incase it isn't Translation2d centerOfModules = moduleLocationsMeters[0].plus(moduleLocationsMeters[1]) - .plus(moduleLocationsMeters[2]).plus(moduleLocationsMeters[3]); + .plus(moduleLocationsMeters[2]) + .plus(moduleLocationsMeters[3]); //Find Largest Radius by checking the distance to the center point - double largestRadius = centerOfModules.getDistance(moduleLocationsMeters[0]); - for(int i=1; i groups = new HashMap(); +/** + * Class for managing persistent alerts to be sent over NetworkTables. + */ +public class Alert +{ - private final AlertType type; - private boolean active = false; - private double activeStartTime = 0.0; - private String text; + private static Map groups = new HashMap(); - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, - * the appropriate entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) { - this("Alerts", text, type); + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate + * entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) + { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate entries will be added to + * NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) + { + if (!groups.containsKey(group)) + { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also be sent to the + * console. + */ + public void set(boolean active) + { + 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; + } } + this.active = active; + } + /** + * Updates current alert text. + */ + public void setText(String text) + { + 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; + } + } + this.text = text; + } + + /** + * Represents an alert's level of urgency. + */ + public static enum AlertType + { /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate - * entries will be added to NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which + * will seriously affect the robot's functionality and thus require immediate attention. */ - public Alert(String group, String text, AlertType type) { - if (!groups.containsKey(group)) { - groups.put(group, new SendableAlerts()); - SmartDashboard.putData(group, groups.get(group)); - } - - this.text = text; - this.type = type; - groups.get(group).alerts.add(this); - } + ERROR, + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which + * will seriously affect the robot's functionality and thus require immediate attention. Trace printed to driver + * station console. + */ + ERROR_TRACE, /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems + * which could affect the robot's functionality but do not necessarily require immediate attention. */ - public void set(boolean active) { - 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; - } - } - this.active = active; - } + WARNING, + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems + * which could affect the robot's functionality but do not necessarily require immediate attention. Trace printed to + * driver station console. + */ + WARNING_TRACE, + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type for problems which + * are unlikely to affect the robot's functionality, or any other alerts which do not fall under "ERROR" or + * "WARNING". + */ + INFO + } - /** Updates current alert text. */ - public void setText(String text) { - 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; - } - } - this.text = text; - } + private static class SendableAlerts implements Sendable + { + + public final List alerts = new ArrayList<>(); - private static class SendableAlerts implements Sendable { - public final List alerts = new ArrayList<>(); - - 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); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } + 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); } - /** Represents an alert's level of urgency. */ - public static enum AlertType { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - */ - ERROR, - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type - * for problems which will seriously affect the robot's functionality and thus require immediate - * attention. - * Trace printed to driver station console. - */ - ERROR_TRACE, - - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - */ - WARNING, - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this - * type for problems which could affect the robot's functionality but do not necessarily require - * immediate attention. - * Trace printed to driver station console. - */ - WARNING_TRACE, - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type - * for problems which are unlikely to affect the robot's functionality, or any other alerts - * which do not fall under "ERROR" or "WARNING". - */ - INFO + @Override + public void initSendable(SendableBuilder builder) + { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); } + } } \ No newline at end of file From a234369e34f655427f853c36e326741e7989b900 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:25:31 -0600 Subject: [PATCH 04/15] Removed unnecessary folder Signed-off-by: thenetworkgrinch --- src/main/deploy/apriltags/2023-chargedup.json | 152 ------------------ 1 file changed, 152 deletions(-) delete mode 100644 src/main/deploy/apriltags/2023-chargedup.json diff --git a/src/main/deploy/apriltags/2023-chargedup.json b/src/main/deploy/apriltags/2023-chargedup.json deleted file mode 100644 index 368081b9..00000000 --- a/src/main/deploy/apriltags/2023-chargedup.json +++ /dev/null @@ -1,152 +0,0 @@ -{ - "tags": [ - { - "ID": 1, - "pose": { - "translation": { - "x": 15.513558, - "y": 1.071626, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 2, - "pose": { - "translation": { - "x": 15.513558, - "y": 2.748026, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 3, - "pose": { - "translation": { - "x": 15.513558, - "y": 4.424426, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 4, - "pose": { - "translation": { - "x": 16.178784, - "y": 6.749796, - "z": 0.695452 - }, - "rotation": { - "quaternion": { - "W": 0.0, - "X": 0.0, - "Y": 0.0, - "Z": 1.0 - } - } - } - }, - { - "ID": 5, - "pose": { - "translation": { - "x": 0.36195, - "y": 6.749796, - "z": 0.695452 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 6, - "pose": { - "translation": { - "x": 1.02743, - "y": 4.424426, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 7, - "pose": { - "translation": { - "x": 1.02743, - "y": 2.748026, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - }, - { - "ID": 8, - "pose": { - "translation": { - "x": 1.02743, - "y": 1.071626, - "z": 0.462788 - }, - "rotation": { - "quaternion": { - "W": 1.0, - "X": 0.0, - "Y": 0.0, - "Z": 0.0 - } - } - } - } - ], - "field": { - "length": 16.54175, - "width": 8.0137 - } -} \ No newline at end of file From 28494dbda2b53c8a0013c80c0ba109240a1d0c2d Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:30:49 -0600 Subject: [PATCH 05/15] Update max swerve files. Signed-off-by: thenetworkgrinch --- .../modules/physicalproperties.json | 21 ------------------- .../controllerproperties.json | 0 .../modules/backleft.json | 0 .../modules/backright.json | 0 .../modules/frontleft.json | 0 .../modules/frontright.json | 0 .../maxSwerve/modules/physicalproperties.json | 16 ++++++++++++++ .../modules/pidfproperties.json | 0 .../swervedrive.json | 4 +--- 9 files changed, 17 insertions(+), 24 deletions(-) delete mode 100644 src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/controllerproperties.json (100%) rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/modules/backleft.json (100%) rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/modules/backright.json (100%) rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/modules/frontleft.json (100%) rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/modules/frontright.json (100%) create mode 100644 src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/modules/pidfproperties.json (100%) rename src/main/deploy/swerve/{MaxSwerve-Neo-NavX => maxSwerve}/swervedrive.json (73%) diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json b/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json deleted file mode 100644 index dd574ad8..00000000 --- a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/physicalproperties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "wheelDiameter": 3, - "gearRatio": { - "drive": 4.714285714285714, - "angle": 1 - }, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - }, - "encoderPulsePerRotation": { - "drive": 1, - "angle": 1 - }, - "wheelGripCoefficientOfFriction": 1.19, - "angleMotorFreeSpeedRPM": 5676 -} \ No newline at end of file diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/controllerproperties.json b/src/main/deploy/swerve/maxSwerve/controllerproperties.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/controllerproperties.json rename to src/main/deploy/swerve/maxSwerve/controllerproperties.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backleft.json b/src/main/deploy/swerve/maxSwerve/modules/backleft.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backleft.json rename to src/main/deploy/swerve/maxSwerve/modules/backleft.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backright.json b/src/main/deploy/swerve/maxSwerve/modules/backright.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/backright.json rename to src/main/deploy/swerve/maxSwerve/modules/backright.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontleft.json b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontleft.json rename to src/main/deploy/swerve/maxSwerve/modules/frontleft.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontright.json b/src/main/deploy/swerve/maxSwerve/modules/frontright.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/frontright.json rename to src/main/deploy/swerve/maxSwerve/modules/frontright.json diff --git a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json new file mode 100644 index 00000000..6cdee294 --- /dev/null +++ b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json @@ -0,0 +1,16 @@ +{ + "conversionFactor": { + "drive": 0.047286787200699704, + "angle": 16.8 + }, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "rampRate": { + "drive": 0.25, + "angle": 0.25 + }, + "optimalVoltage": 12, + "wheelGripCoefficientOfFriction": 1.19, +} \ No newline at end of file diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/pidfproperties.json b/src/main/deploy/swerve/maxSwerve/modules/pidfproperties.json similarity index 100% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/modules/pidfproperties.json rename to src/main/deploy/swerve/maxSwerve/modules/pidfproperties.json diff --git a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json b/src/main/deploy/swerve/maxSwerve/swervedrive.json similarity index 73% rename from src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json rename to src/main/deploy/swerve/maxSwerve/swervedrive.json index dd4d3253..b42e51e0 100644 --- a/src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json +++ b/src/main/deploy/swerve/maxSwerve/swervedrive.json @@ -1,12 +1,10 @@ { - "maxSpeed": 14.5, - "optimalVoltage": 12, "imu": { "type": "navx_spi", "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", From 131f87db909a790eb23c9a61f12948e60ba712d2 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:31:14 -0600 Subject: [PATCH 06/15] Fixed json Signed-off-by: thenetworkgrinch --- .../deploy/swerve/maxSwerve/modules/physicalproperties.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json index 6cdee294..c3a74085 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json @@ -12,5 +12,5 @@ "angle": 0.25 }, "optimalVoltage": 12, - "wheelGripCoefficientOfFriction": 1.19, + "wheelGripCoefficientOfFriction": 1.19 } \ No newline at end of file From f6d2e91cf194dbed1cd581ef344cb36b28d58a13 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:32:20 -0600 Subject: [PATCH 07/15] Fixed the conversion factor Signed-off-by: thenetworkgrinch --- .../deploy/swerve/maxSwerve/modules/physicalproperties.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json index c3a74085..55a1388d 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "conversionFactor": { "drive": 0.047286787200699704, - "angle": 16.8 + "angle": 360 }, "currentLimit": { "drive": 40, From 35ff0b81637633f5ad88412f0eda16832a3d3997 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 22:15:24 -0600 Subject: [PATCH 08/15] Fixed alert's to prevent null pointer dereference. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/SwerveModule.java | 31 +++++++------- .../encoders/AnalogAbsoluteEncoderSwerve.java | 20 +++++----- .../swervelib/encoders/CANCoderSwerve.java | 40 ++++++++++--------- .../encoders/PWMDutyCycleEncoderSwerve.java | 11 ++--- .../encoders/SparkMaxAnalogEncoderSwerve.java | 19 +++++---- .../encoders/SparkMaxEncoderSwerve.java | 21 ++++++---- src/main/java/swervelib/imu/NavXSwerve.java | 5 ++- .../swervelib/motors/SparkFlexSwerve.java | 21 +++++----- .../motors/SparkMaxBrushedMotorSwerve.java | 24 ++++++----- .../java/swervelib/motors/TalonFXSwerve.java | 10 ++--- .../swervelib/parser/json/DeviceJson.java | 28 ++++++------- 11 files changed, 128 insertions(+), 102 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 61d2b590..fe3989aa 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -33,6 +33,14 @@ public class SwerveModule * Absolute encoder for swerve drive. */ private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private final Alert encoderOffsetWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert noEncoderWarning; /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ @@ -61,20 +69,6 @@ public class SwerveModule * Encoder synchronization queued. */ private boolean synchronizeEncoderQueued = false; - /** - * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. - */ - private Alert encoderOffsetWarning = new Alert("Motors", - "Pushing the Absolute Encoder offset to the encoder failed on module #" + - moduleNumber, - Alert.AlertType.WARNING); - /** - * An {@link Alert} for if there is no Absolute Encoder on the module. - */ - private Alert noEncoderWarning = new Alert("Motors", - "There is no Absolute Encoder on module #" + - moduleNumber, - Alert.AlertType.WARNING); /** * Construct the swerve module and initialize the swerve module motors and absolute encoder. @@ -143,6 +137,15 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat } lastState = getState(); + + noEncoderWarning = new Alert("Motors", + "There is no Absolute Encoder on module #" + + moduleNumber, + Alert.AlertType.WARNING); + encoderOffsetWarning = new Alert("Motors", + "Pushing the Absolute Encoder offset to the encoder failed on module #" + + moduleNumber, + Alert.AlertType.WARNING); } /** diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index f0af1a03..bcf4df95 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -18,21 +18,15 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder /** * Inversion state of the encoder. */ - private boolean inverted = false; + private boolean inverted = false; /** * An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset = new Alert( - "Encoders", - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), - Alert.AlertType.WARNING); + private Alert cannotSetOffset; /** * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */ - private Alert inaccurateVelocities = new Alert( - "Encoders", - "The Analog Absolute encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); + private Alert inaccurateVelocities; /** * Construct the Thrifty Encoder as a Swerve Absolute Encoder. @@ -42,6 +36,14 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) { this.encoder = encoder; + cannotSetOffset = new Alert( + "Encoders", + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + Alert.AlertType.WARNING); + inaccurateVelocities = new Alert( + "Encoders", + "The Analog Absolute encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); } /** diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 7a71875e..011560c7 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -24,33 +24,19 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if the CANCoder magnet field is less than ideal. */ - private Alert magnetFieldLessThanIdeal = new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", - Alert.AlertType.WARNING); + private Alert magnetFieldLessThanIdeal; /** * An {@link Alert} for if the CANCoder reading is faulty. */ - private Alert readingFaulty = new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty.", - Alert.AlertType.WARNING); + private Alert readingFaulty; /** * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ - private Alert readingIgnored = new Alert( - "Encoders", - "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", - Alert.AlertType.WARNING); + private Alert readingIgnored; /** * An {@link Alert} for if the absolute encoder offset cannot be set. */ - private Alert cannotSetOffset = new Alert( - "Encoders", - "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset", - Alert.AlertType.WARNING); + private Alert cannotSetOffset; /** * Initialize the CANCoder on the standard CANBus. @@ -71,6 +57,24 @@ public CANCoderSwerve(int id) public CANCoderSwerve(int id, String canbus) { encoder = new CANcoder(id, canbus); + magnetFieldLessThanIdeal = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + readingFaulty = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + readingIgnored = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + cannotSetOffset = new Alert( + "Encoders", + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset", + Alert.AlertType.WARNING); } /** diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index d43c80ed..24e15d66 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -25,11 +25,7 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if the encoder cannot report accurate velocities. */ - private Alert inaccurateVelocities = new Alert( - "Encoders", - "The PWM Duty Cycle encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); - + private Alert inaccurateVelocities; /** * Constructor for the PWM duty cycle encoder. @@ -39,6 +35,11 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder public PWMDutyCycleEncoderSwerve(int pin) { encoder = new DutyCycleEncoder(pin); + inaccurateVelocities = new Alert( + "Encoders", + "The PWM Duty Cycle encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); + } /** diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 22084d32..30247879 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -21,17 +21,11 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring = new Alert( - "Encoders", - "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); + private Alert failureConfiguring; /** * An {@link Alert} for if the absolute encoder does not support integrated offsets. */ - private Alert doesNotSupportIntegratedOffsets = new Alert( - "Encoders", - "SparkMax Analog Sensors do not support integrated offsets", - Alert.AlertType.WARNING_TRACE); + private Alert doesNotSupportIntegratedOffsets; /** @@ -49,6 +43,15 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor) { 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); + doesNotSupportIntegratedOffsets = new Alert( + "Encoders", + "SparkMax Analog Sensors do not support integrated offsets", + Alert.AlertType.WARNING_TRACE); + } /** diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index 4d8ed8e2..dd81adae 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -21,14 +21,11 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring = new Alert( - "Encoders", - "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); - private Alert offsetFailure = new Alert( - "Encoders", - "Failure to set Absolute Encoder Offset", - Alert.AlertType.WARNING_TRACE); + private Alert failureConfiguring; + /** + * An {@link Alert} for if there is a failure configuring the encoder offset. + */ + private Alert offsetFailure; /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. @@ -47,6 +44,14 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) { 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/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 8fa09fdb..339c0e97 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -23,11 +23,11 @@ public class NavXSwerve extends SwerveIMU /** * Offset for the NavX. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * An {@link Alert} for if there is an error instantiating the NavX. */ - private Alert navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); + private Alert navXError; /** * Constructor for the NavX swerve. @@ -36,6 +36,7 @@ public class NavXSwerve extends SwerveIMU */ public NavXSwerve(SerialPort.Port port) { + navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); try { /* Communicate w/navX-MXP via the MXP SPI Bus. */ diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 22216fe4..27c9c4a9 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -43,21 +43,15 @@ public class SparkFlexSwerve extends SwerveMotor /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguring = new Alert("Motors", - "Failure configuring motor " + - motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); + private Alert failureConfiguring; /** * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ - private Alert absoluteEncoderOffsetWarning = new Alert("Motors", - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + - "absoluteEncoderOffset in the Swerve Module JSON!", - Alert.AlertType.WARNING); + private Alert absoluteEncoderOffsetWarning; /** * Initialize the swerve motor. @@ -79,6 +73,15 @@ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + failureConfiguring = new Alert("Motors", + "Failure configuring motor " + + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + absoluteEncoderOffsetWarning = new Alert("Motors", + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + + "absoluteEncoderOffset in the Swerve Module JSON!", + Alert.AlertType.WARNING); + } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 6d461594..3ec65369 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -42,25 +42,19 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if the motor has no encoder. */ - private Alert noEncoderAlert = new Alert("Motors", - "Cannot use motor without encoder.", - Alert.AlertType.ERROR_TRACE); + private Alert noEncoderAlert; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguringAlert = new Alert("Motors", - "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); + private Alert failureConfiguringAlert; /** * An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert = new Alert("Motors", - "An encoder MUST be defined to work with a SparkMAX", - Alert.AlertType.ERROR_TRACE); + private Alert noEncoderDefinedAlert; /** * Initialize the swerve motor. @@ -108,6 +102,16 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type } // 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/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 2deefb35..fab3a016 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -20,19 +20,19 @@ public class TalonFXSwerve extends SwerveMotor /** * Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; + private final boolean factoryDefaultOccurred = false; /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private final boolean absoluteEncoder = false; /** * Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); /** * Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); // /** // * Motion Magic exponential voltage setters. // */ @@ -44,7 +44,7 @@ public class TalonFXSwerve extends SwerveMotor /** * Current TalonFX configuration. */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 4320f3a6..59e12053 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -35,29 +35,29 @@ public class DeviceJson { /** - * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx + * An {@link Alert} for if the CAN ID is greater than 40. */ - public String type; + 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); /** - * The CAN ID or pin ID of the device. + * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. */ - public int id; + private final Alert i2cLockupWarning = new Alert("IMU", + "I2C lockup issue detected on roboRIO. Check console for more information.", + Alert.AlertType.WARNING); /** - * The CAN bus name which the device resides on if using CAN. + * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ - public String canbus = ""; + public String type; /** - * An {@link Alert} for if the CAN ID is greater than 40. + * The CAN ID or pin ID of the device. */ - private Alert canIdWarning = new Alert("JSON", - "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - Alert.AlertType.WARNING); + public int id; /** - * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. + * The CAN bus name which the device resides on if using CAN. */ - private Alert i2cLockupWarning = new Alert("IMU", - "I2C lockup issue detected on roboRIO. Check console for more information.", - Alert.AlertType.WARNING); + public String canbus = ""; /** * Create a {@link SwerveAbsoluteEncoder} from the current configuration. From be1b79b712de39844a6673e5fc363fc0ce6e643b Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 15:27:21 -0600 Subject: [PATCH 09/15] Fixed the conversion factor for talonfx's. Added new command generators. Signed-off-by: thenetworkgrinch --- .../falcon/modules/physicalproperties.json | 4 +- .../swervedrive/SwerveSubsystem.java | 59 ++++++++++++++++--- .../java/swervelib/motors/TalonFXSwerve.java | 2 +- 3 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/swerve/falcon/modules/physicalproperties.json b/src/main/deploy/swerve/falcon/modules/physicalproperties.json index d09b181b..8d11c027 100644 --- a/src/main/deploy/swerve/falcon/modules/physicalproperties.json +++ b/src/main/deploy/swerve/falcon/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "conversionFactor": { - "angle": 0.01373291015625, - "drive": 1.914649238933429E-5 + "angle": 28.125, + "drive": 0.047286787200699704 }, "currentLimit": { "drive": 40, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 35a38191..0e887a48 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.io.File; +import java.util.function.DoubleSupplier; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.math.SwerveMath; @@ -76,6 +78,7 @@ public SwerveSubsystem(File directory) } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setGyroOffset(new Rotation3d(0, 0, Units.degreesToRadians(90))); setupPathPlanner(); } @@ -104,12 +107,12 @@ public void setupPathPlanner() // Default path replanning config. See the API for the options here ), () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - }, + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + }, this // Reference to this subsystem to set requirements ); } @@ -135,6 +138,45 @@ public Command getAutonomousCommand(String pathName, boolean setOdomToStart) return AutoBuilder.followPath(path); } + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, + DoubleSupplier headingY) + { + return run(() -> { + // Make the robot move + driveFieldOriented(getTargetSpeeds(translationX.getAsDouble(), translationY.getAsDouble(), + headingX.getAsDouble(), + headingY.getAsDouble())); + }); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. + * @param translationY Translation in the Y direction. + * @param angularRotationX Rotation of the robot to set + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) + { + return run(() -> { + // Make the robot move + swerveDrive.drive(new Translation2d(translationX.getAsDouble() * maximumSpeed, translationY.getAsDouble()), + angularRotationX.getAsDouble() * swerveDrive.swerveController.config.maxAngularVelocity, + true, + false); + }); + } + /** * Construct the swerve drive. * @@ -301,7 +343,8 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin } /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of + * 90deg. * * @param xInput X joystick input for the robot to move in the X direction. * @param yInput Y joystick input for the robot to move in the Y direction. @@ -315,7 +358,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), - getHeading().getRadians(), + getHeading().getRadians() - Units.degreesToRadians(90), maximumSpeed); } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index fab3a016..e112d606 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -154,7 +154,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); - positionConversionFactor = 1 / positionConversionFactor; + // positionConversionFactor = 1 / positionConversionFactor; configuration.MotionMagic = configuration.MotionMagic .withMotionMagicCruiseVelocity(100 / positionConversionFactor) From c4440f96dd6a27fca44ce6957deb9042297b8c2d Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 15:31:23 -0600 Subject: [PATCH 10/15] Added double precision. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/motors/TalonFXSwerve.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index e112d606..d0b3c3fb 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -157,8 +157,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) // positionConversionFactor = 1 / positionConversionFactor; configuration.MotionMagic = configuration.MotionMagic - .withMotionMagicCruiseVelocity(100 / positionConversionFactor) - .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) + .withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) .withMotionMagicExpo_kV(0.12 * positionConversionFactor) .withMotionMagicExpo_kA(0.1); From 4d9cb06a233039e3b30bb4f69690e11a1d5e2ec1 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 15:48:34 -0600 Subject: [PATCH 11/15] Use voltage setter instead. Signed-off-by: thenetworkgrinch --- .../java/swervelib/motors/TalonFXSwerve.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index d0b3c3fb..a258e746 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -24,15 +25,15 @@ public class TalonFXSwerve extends SwerveMotor /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private final boolean absoluteEncoder = false; /** - * Motion magic angle voltage setter. + * Position angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + private final PositionVoltage m_angleVoltageSetter = new PositionVoltage(0); /** * Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); // /** // * Motion Magic exponential voltage setters. // */ @@ -156,11 +157,11 @@ public void configureIntegratedEncoder(double positionConversionFactor) // positionConversionFactor = 1 / positionConversionFactor; - configuration.MotionMagic = configuration.MotionMagic - .withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) - .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) - .withMotionMagicExpo_kV(0.12 * positionConversionFactor) - .withMotionMagicExpo_kA(0.1); +// configuration.MotionMagic = configuration.MotionMagic +// .withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) +// .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) +// .withMotionMagicExpo_kV(0.12 * positionConversionFactor) +// .withMotionMagicExpo_kA(0.1); configuration.Feedback = configuration.Feedback .withSensorToMechanismRatio(positionConversionFactor) From 1e011e00c0e1d465cf30394bb98f897f30183453 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 16:43:18 -0600 Subject: [PATCH 12/15] Removed offset example. Signed-off-by: thenetworkgrinch --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 0e887a48..a8e76451 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -78,7 +78,6 @@ public SwerveSubsystem(File directory) } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setGyroOffset(new Rotation3d(0, 0, Units.degreesToRadians(90))); setupPathPlanner(); } @@ -358,7 +357,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), - getHeading().getRadians() - Units.degreesToRadians(90), + getHeading().getRadians(), maximumSpeed); } From 5d39cb3798fa19ac1886e23f029dc04897d49132 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 16:58:15 -0600 Subject: [PATCH 13/15] Motion magic again! Signed-off-by: thenetworkgrinch --- .../swerve/falcon/modules/pidfproperties.json | 8 +- .../java/swervelib/motors/TalonFXSwerve.java | 303 ++++++++---------- 2 files changed, 136 insertions(+), 175 deletions(-) diff --git a/src/main/deploy/swerve/falcon/modules/pidfproperties.json b/src/main/deploy/swerve/falcon/modules/pidfproperties.json index 81bae4b0..b562c2b1 100644 --- a/src/main/deploy/swerve/falcon/modules/pidfproperties.json +++ b/src/main/deploy/swerve/falcon/modules/pidfproperties.json @@ -1,15 +1,15 @@ { "drive": { - "p": 0.15, + "p": 1, "i": 0, - "d": 2.0, + "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.050953, + "p": 360, "i": 0, - "d": 0.118, + "d": 0, "f": 0, "iz": 0 } diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index a258e746..f1dd73d7 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; @@ -12,113 +11,88 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** - * {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. - */ -public class TalonFXSwerve extends SwerveMotor -{ +/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ +public class TalonFXSwerve extends SwerveMotor { - /** - * Factory default already occurred. - */ - private final boolean factoryDefaultOccurred = false; - /** - * Whether the absolute encoder is integrated. - */ - private final boolean absoluteEncoder = false; - /** - * Position angle voltage setter. - */ - private final PositionVoltage m_angleVoltageSetter = new PositionVoltage(0); - /** - * Velocity voltage setter for controlling drive motor. - */ + /** Factory default already occurred. */ + private final boolean factoryDefaultOccurred = false; + /** Whether the absolute encoder is integrated. */ + private final boolean absoluteEncoder = false; + /** Motion magic angle voltage setter. */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + /** Velocity voltage setter for controlling drive motor. */ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); -// /** -// * Motion Magic exponential voltage setters. -// */ -// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); - /** - * TalonFX motor controller. - */ + // /** + // * Motion Magic exponential voltage setters. + // */ + // private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); + /** TalonFX motor controller. */ TalonFX motor; - /** - * Current TalonFX configuration. - */ + /** Current TalonFX configuration. */ private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. * - * @param motor Motor to use. + * @param motor Motor to use. * @param isDriveMotor Whether this motor is a drive motor. */ - public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) - { + public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { this.isDriveMotor = isDriveMotor; this.motor = motor; factoryDefaults(); clearStickyFaults(); -// if (SwerveDriveTelemetry.isSimulation) -// { -//// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); -// } + // if (SwerveDriveTelemetry.isSimulation) + // { + //// PhysicsSim.getInstance().addTalonFX(motor, .25, 6800); + // } } /** * Construct the TalonFX swerve motor given the ID and CANBus. * - * @param id ID of the TalonFX on the CANBus. - * @param canbus CANBus on which the TalonFX is on. + * @param id ID of the TalonFX on the CANBus. + * @param canbus CANBus on which the TalonFX is on. * @param isDriveMotor Whether the motor is a drive or steering motor. */ - public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) - { + public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) { this(new TalonFX(id, canbus), isDriveMotor); } /** * Construct the TalonFX swerve motor given the ID. * - * @param id ID of the TalonFX on the canbus. + * @param id ID of the TalonFX on the canbus. * @param isDriveMotor Whether the motor is a drive or steering motor. */ - public TalonFXSwerve(int id, boolean isDriveMotor) - { + public TalonFXSwerve(int id, boolean isDriveMotor) { this(new TalonFX(id), isDriveMotor); } - /** - * Configure the factory defaults. - */ + /** Configure the factory defaults. */ @Override - public void factoryDefaults() - { - if (!factoryDefaultOccurred) - { + public void factoryDefaults() { + if (!factoryDefaultOccurred) { TalonFXConfigurator cfg = motor.getConfigurator(); configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; configuration.ClosedLoopGeneral.ContinuousWrap = true; cfg.apply(configuration); m_angleVoltageSetter.UpdateFreqHz = 0; -// m_angleVoltageExpoSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; m_velocityVoltageSetter.UpdateFreqHz = 0; -// motor.configFactoryDefault(); -// motor.setSensorPhase(true); -// motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); -// motor.configNeutralDeadband(0.001); + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); } } - /** - * Clear the sticky faults on the motor controller. - */ + /** Clear the sticky faults on the motor controller. */ @Override - public void clearStickyFaults() - { + public void clearStickyFaults() { motor.clearStickyFaults(); } @@ -128,44 +102,43 @@ public void clearStickyFaults() * @param encoder The encoder to use. */ @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) - { + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { // Do not support. return this; } /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * Configure the integrated encoder for the swerve module. Sets the conversion factors for + * position and velocity. * * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * + *


+ * Degrees:
+ * * 360 / (angleGearRatio * encoderTicksPerRotation) *
- *


- * Meters:
- * + *


+ * Meters:
+ * * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) * */ @Override - public void configureIntegratedEncoder(double positionConversionFactor) - { + public void configureIntegratedEncoder(double positionConversionFactor) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); - // positionConversionFactor = 1 / positionConversionFactor; +// positionConversionFactor = 1 / positionConversionFactor; -// configuration.MotionMagic = configuration.MotionMagic -// .withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) -// .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) -// .withMotionMagicExpo_kV(0.12 * positionConversionFactor) -// .withMotionMagicExpo_kA(0.1); + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100 / positionConversionFactor) + .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); - configuration.Feedback = configuration.Feedback - .withSensorToMechanismRatio(positionConversionFactor) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + configuration.Feedback = + configuration.Feedback.withSensorToMechanismRatio(positionConversionFactor) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); cfg.apply(configuration); // Taken from democat's library. @@ -178,42 +151,51 @@ public void configureIntegratedEncoder(double positionConversionFactor) * * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information */ - public void configureCANStatusFrames(int CANStatus1) - { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + public void configureCANStatusFrames(int CANStatus1) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); } /** * Set the CAN status frames. * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current - * Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed + * Supply Current Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current + * Measurement */ - public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus8, - int CANStatus10, int CANStatus12, int CANStatus13, int CANStatus14, - int CANStatus21, int CANStatusCurrent) - { -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); -// motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, CANStatusCurrent); + public void configureCANStatusFrames( + int CANStatus1, + int CANStatus2, + int CANStatus3, + int CANStatus4, + int CANStatus8, + int CANStatus10, + int CANStatus12, + int CANStatus13, + int CANStatus14, + int CANStatus21, + int CANStatusCurrent) { + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); + // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, + // CANStatusCurrent); // TODO: Configure Status Frame 2 thru 21 if necessary // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods @@ -225,16 +207,13 @@ public void configureCANStatusFrames(int CANStatus1, int CANStatus2, int CANStat * @param config Configuration class holding the PIDF values. */ @Override - public void configurePIDF(PIDFConfig config) - { + public void configurePIDF(PIDFConfig config) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); - cfg.apply(configuration.Slot0.withKP(config.p) - .withKI(config.i) - .withKD(config.d) - .withKS(config.f)); -// configuration.slot0.integralZone = config.iz; -// configuration.slot0.closedLoopPeakOutput = config.output.max; + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; } /** @@ -244,8 +223,7 @@ public void configurePIDF(PIDFConfig config) * @param maxInput Maximum PID input. */ @Override - public void configurePIDWrapping(double minInput, double maxInput) - { + public void configurePIDWrapping(double minInput, double maxInput) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopGeneral); configuration.ClosedLoopGeneral.ContinuousWrap = true; @@ -258,8 +236,7 @@ public void configurePIDWrapping(double minInput, double maxInput) * @param isBrakeMode Set the brake mode. */ @Override - public void setMotorBrake(boolean isBrakeMode) - { + public void setMotorBrake(boolean isBrakeMode) { motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); } @@ -269,18 +246,14 @@ public void setMotorBrake(boolean isBrakeMode) * @param inverted State of inversion. */ @Override - public void setInverted(boolean inverted) - { -// Timer.delay(1); + public void setInverted(boolean inverted) { + // Timer.delay(1); motor.setInverted(inverted); } - /** - * Save the configurations from flash to EEPROM. - */ + /** Save the configurations from flash to EEPROM. */ @Override - public void burnFlash() - { + public void burnFlash() { // Do nothing } @@ -290,43 +263,38 @@ public void burnFlash() * @param percentOutput percent out for the motor controller. */ @Override - public void set(double percentOutput) - { + 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 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) - { + public void setReference(double setpoint, double feedforward) { setReference(setpoint, feedforward, getPosition()); } /** * Set the closed loop PID controller reference point. * - * @param setpoint Setpoint in meters per second or angle in degrees. + * @param setpoint Setpoint in meters per second or angle in degrees. * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. + * @param position Only used on the angle motor, the position of the motor in degrees. */ @Override - public void setReference(double setpoint, double feedforward, double position) - { -// if (SwerveDriveTelemetry.isSimulation) -// { -// PhysicsSim.getInstance().run(); -// } - - if (isDriveMotor) - { + public void setReference(double setpoint, double feedforward, double position) { + // if (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } + + if (isDriveMotor) { motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); - } else - { + } else { motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); } } @@ -337,8 +305,7 @@ public void setReference(double setpoint, double feedforward, double position) * @return velocity in Meters Per Second, or Degrees per Second. */ @Override - public double getVelocity() - { + public double getVelocity() { return motor.getVelocity().getValue(); } @@ -348,8 +315,7 @@ public double getVelocity() * @return Position in Meters or Degrees. */ @Override - public double getPosition() - { + public double getPosition() { return motor.getPosition().getValue(); } @@ -359,10 +325,8 @@ public double getPosition() * @param position Integrated encoder position. Should be angle in degrees or meters. */ @Override - public void setPosition(double position) - { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) - { + public void setPosition(double position) { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { position = position < 0 ? (position % 360) + 360 : position; TalonFXConfigurator cfg = motor.getConfigurator(); cfg.setPosition(position); @@ -375,23 +339,23 @@ public void setPosition(double position) * @param nominalVoltage Nominal voltage for operation to output to. */ @Override - public void setVoltageCompensation(double nominalVoltage) - { + public void setVoltageCompensation(double nominalVoltage) { // Do not implement } /** - * 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. + * 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) - { + public void setCurrentLimit(int currentLimit) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); - cfg.apply(configuration.CurrentLimits.withStatorCurrentLimit(currentLimit).withStatorCurrentLimitEnable(true)); + cfg.apply( + configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + .withStatorCurrentLimitEnable(true)); } /** @@ -400,8 +364,7 @@ public void setCurrentLimit(int currentLimit) * @param rampRate Time in seconds to go from 0 to full throttle. */ @Override - public void setLoopRampRate(double rampRate) - { + public void setLoopRampRate(double rampRate) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopRamps); cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); @@ -413,8 +376,7 @@ public void setLoopRampRate(double rampRate) * @return Motor object. */ @Override - public Object getMotor() - { + public Object getMotor() { return motor; } @@ -424,8 +386,7 @@ public Object getMotor() * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() - { + public boolean isAttachedAbsoluteEncoder() { return absoluteEncoder; } } From 2ca9fbd4907a4819f1125bf12ed8fb499a71f960 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 17:02:44 -0600 Subject: [PATCH 14/15] Double precision Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/motors/TalonFXSwerve.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index f1dd73d7..645ffef4 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -131,8 +131,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) { // positionConversionFactor = 1 / positionConversionFactor; configuration.MotionMagic = - configuration.MotionMagic.withMotionMagicCruiseVelocity(100 / positionConversionFactor) - .withMotionMagicAcceleration((100 / positionConversionFactor) / 0.100) + configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) .withMotionMagicExpo_kV(0.12 * positionConversionFactor) .withMotionMagicExpo_kA(0.1); From 544d0064e97c2cf38882252823186b27c3b4df2f Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 17 Jan 2024 20:46:35 -0600 Subject: [PATCH 15/15] Format TalonFX and use rotor rotations instead. Signed-off-by: thenetworkgrinch --- .../java/swervelib/motors/TalonFXSwerve.java | 223 +++++++++++------- 1 file changed, 139 insertions(+), 84 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 645ffef4..a5c0e22b 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -11,33 +11,49 @@ import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; -/** {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. */ -public class TalonFXSwerve extends SwerveMotor { - - /** Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - /** Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; - /** Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); - /** Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - // /** - // * Motion Magic exponential voltage setters. - // */ - // private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); - /** TalonFX motor controller. */ +/** + * {@link com.ctre.phoenix6.hardware.TalonFX} Swerve Motor. Made by Team 1466 WebbRobotics. + */ +public class TalonFXSwerve extends SwerveMotor +{ + + /** + * Factory default already occurred. + */ + private final boolean factoryDefaultOccurred = false; + /** + * Whether the absolute encoder is integrated. + */ + private final boolean absoluteEncoder = false; + /** + * Motion magic angle voltage setter. + */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + /** + * Velocity voltage setter for controlling drive motor. + */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + /** + * Conversion factor for the motor. + */ + private double conversionFactor; + /** + * TalonFX motor controller. + */ TalonFX motor; - /** Current TalonFX configuration. */ + /** + * Current TalonFX configuration. + */ private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. * - * @param motor Motor to use. + * @param motor Motor to use. * @param isDriveMotor Whether this motor is a drive motor. */ - public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { + public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) + { this.isDriveMotor = isDriveMotor; this.motor = motor; @@ -53,28 +69,34 @@ public TalonFXSwerve(TalonFX motor, boolean isDriveMotor) { /** * Construct the TalonFX swerve motor given the ID and CANBus. * - * @param id ID of the TalonFX on the CANBus. - * @param canbus CANBus on which the TalonFX is on. + * @param id ID of the TalonFX on the CANBus. + * @param canbus CANBus on which the TalonFX is on. * @param isDriveMotor Whether the motor is a drive or steering motor. */ - public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) { + public TalonFXSwerve(int id, String canbus, boolean isDriveMotor) + { this(new TalonFX(id, canbus), isDriveMotor); } /** * Construct the TalonFX swerve motor given the ID. * - * @param id ID of the TalonFX on the canbus. + * @param id ID of the TalonFX on the canbus. * @param isDriveMotor Whether the motor is a drive or steering motor. */ - public TalonFXSwerve(int id, boolean isDriveMotor) { + public TalonFXSwerve(int id, boolean isDriveMotor) + { this(new TalonFX(id), isDriveMotor); } - /** Configure the factory defaults. */ + /** + * Configure the factory defaults. + */ @Override - public void factoryDefaults() { - if (!factoryDefaultOccurred) { + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { TalonFXConfigurator cfg = motor.getConfigurator(); configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; configuration.ClosedLoopGeneral.ContinuousWrap = true; @@ -90,9 +112,12 @@ public void factoryDefaults() { } } - /** Clear the sticky faults on the motor controller. */ + /** + * Clear the sticky faults on the motor controller. + */ @Override - public void clearStickyFaults() { + public void clearStickyFaults() + { motor.clearStickyFaults(); } @@ -102,33 +127,39 @@ public void clearStickyFaults() { * @param encoder The encoder to use. */ @Override - public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { // Do not support. return this; } /** - * Configure the integrated encoder for the swerve module. Sets the conversion factors for - * position and velocity. + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. * * @param positionConversionFactor The conversion factor to apply for position. - *


- * Degrees:
- * + *


+ * Degrees:
+ * * 360 / (angleGearRatio * encoderTicksPerRotation) *
- *


- * Meters:
- * + *


+ * Meters:
+ * * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) * */ @Override - public void configureIntegratedEncoder(double positionConversionFactor) { + public void configureIntegratedEncoder(double positionConversionFactor) + { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration); -// positionConversionFactor = 1 / positionConversionFactor; + positionConversionFactor = 1 / positionConversionFactor; + if (!isDriveMotor) + { + positionConversionFactor *= 360; + } + conversionFactor = positionConversionFactor; configuration.MotionMagic = configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) @@ -136,9 +167,11 @@ public void configureIntegratedEncoder(double positionConversionFactor) { .withMotionMagicExpo_kV(0.12 * positionConversionFactor) .withMotionMagicExpo_kA(0.1); - configuration.Feedback = - configuration.Feedback.withSensorToMechanismRatio(positionConversionFactor) - .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + configuration.Feedback = configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor); + if (isDriveMotor) + { + configuration.Feedback = configuration.Feedback.withSensorToMechanismRatio(positionConversionFactor); + } cfg.apply(configuration); // Taken from democat's library. @@ -151,26 +184,26 @@ public void configureIntegratedEncoder(double positionConversionFactor) { * * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information */ - public void configureCANStatusFrames(int CANStatus1) { + public void configureCANStatusFrames(int CANStatus1) + { // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); } /** * Set the CAN status frames. * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed - * Supply Current Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current - * Measurement + * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information + * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current + * Measurement, Sticky Fault Information + * @param CANStatus3 Quadrature Information + * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature + * @param CANStatus8 Pulse Width Information + * @param CANStatus10 Motion Profiling/Motion Magic Information + * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) + * @param CANStatus13 PID0 (Primary PID) Information + * @param CANStatus14 PID1 (Auxiliary PID) Information + * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) + * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement */ public void configureCANStatusFrames( int CANStatus1, @@ -183,7 +216,8 @@ public void configureCANStatusFrames( int CANStatus13, int CANStatus14, int CANStatus21, - int CANStatusCurrent) { + int CANStatusCurrent) + { // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); @@ -207,7 +241,8 @@ public void configureCANStatusFrames( * @param config Configuration class holding the PIDF values. */ @Override - public void configurePIDF(PIDFConfig config) { + public void configurePIDF(PIDFConfig config) + { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.Slot0); cfg.apply( @@ -223,7 +258,8 @@ public void configurePIDF(PIDFConfig config) { * @param maxInput Maximum PID input. */ @Override - public void configurePIDWrapping(double minInput, double maxInput) { + public void configurePIDWrapping(double minInput, double maxInput) + { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopGeneral); configuration.ClosedLoopGeneral.ContinuousWrap = true; @@ -236,7 +272,8 @@ public void configurePIDWrapping(double minInput, double maxInput) { * @param isBrakeMode Set the brake mode. */ @Override - public void setMotorBrake(boolean isBrakeMode) { + public void setMotorBrake(boolean isBrakeMode) + { motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); } @@ -246,14 +283,18 @@ public void setMotorBrake(boolean isBrakeMode) { * @param inverted State of inversion. */ @Override - public void setInverted(boolean inverted) { + public void setInverted(boolean inverted) + { // Timer.delay(1); motor.setInverted(inverted); } - /** Save the configurations from flash to EEPROM. */ + /** + * Save the configurations from flash to EEPROM. + */ @Override - public void burnFlash() { + public void burnFlash() + { // Do nothing } @@ -263,39 +304,44 @@ public void burnFlash() { * @param percentOutput percent out for the motor controller. */ @Override - public void set(double percentOutput) { + 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 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) { + public void setReference(double setpoint, double feedforward) + { setReference(setpoint, feedforward, getPosition()); } /** * Set the closed loop PID controller reference point. * - * @param setpoint Setpoint in meters per second or angle in degrees. + * @param setpoint Setpoint in meters per second or angle in degrees. * @param feedforward Feedforward in volt-meter-per-second or kV. - * @param position Only used on the angle motor, the position of the motor in degrees. + * @param position Only used on the angle motor, the position of the motor in degrees. */ @Override - public void setReference(double setpoint, double feedforward, double position) { + public void setReference(double setpoint, double feedforward, double position) + { // if (SwerveDriveTelemetry.isSimulation) // { // PhysicsSim.getInstance().run(); // } - if (isDriveMotor) { + if (isDriveMotor) + { motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); - } else { - motor.setControl(m_angleVoltageSetter.withPosition(setpoint)); + } else + { + motor.setControl(m_angleVoltageSetter.withPosition((setpoint - 180) / conversionFactor)); } } @@ -305,7 +351,8 @@ public void setReference(double setpoint, double feedforward, double position) { * @return velocity in Meters Per Second, or Degrees per Second. */ @Override - public double getVelocity() { + public double getVelocity() + { return motor.getVelocity().getValue(); } @@ -315,7 +362,8 @@ public double getVelocity() { * @return Position in Meters or Degrees. */ @Override - public double getPosition() { + public double getPosition() + { return motor.getPosition().getValue(); } @@ -325,8 +373,10 @@ public double getPosition() { * @param position Integrated encoder position. Should be angle in degrees or meters. */ @Override - public void setPosition(double position) { - if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { + public void setPosition(double position) + { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) + { position = position < 0 ? (position % 360) + 360 : position; TalonFXConfigurator cfg = motor.getConfigurator(); cfg.setPosition(position); @@ -339,18 +389,20 @@ public void setPosition(double position) { * @param nominalVoltage Nominal voltage for operation to output to. */ @Override - public void setVoltageCompensation(double nominalVoltage) { + public void setVoltageCompensation(double nominalVoltage) + { // Do not implement } /** - * 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. + * 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) { + public void setCurrentLimit(int currentLimit) + { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); cfg.apply( @@ -364,7 +416,8 @@ public void setCurrentLimit(int currentLimit) { * @param rampRate Time in seconds to go from 0 to full throttle. */ @Override - public void setLoopRampRate(double rampRate) { + public void setLoopRampRate(double rampRate) + { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.ClosedLoopRamps); cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); @@ -376,7 +429,8 @@ public void setLoopRampRate(double rampRate) { * @return Motor object. */ @Override - public Object getMotor() { + public Object getMotor() + { return motor; } @@ -386,7 +440,8 @@ public Object getMotor() { * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() { + public boolean isAttachedAbsoluteEncoder() + { return absoluteEncoder; } }