Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed NetworkAlerts

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-17 09:17:39 -06:00
parent f16ca43981
commit 1ca3db242c
19 changed files with 510 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
swervelib/SwerveDrive.java
View File

@@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Rotation3d;
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;
@@ -67,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.
*/
@@ -100,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.
*/
@@ -120,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.
*/
@@ -413,10 +412,10 @@ public class SwerveDrive
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;

24
swervelib/SwerveModule.java
View File

@@ -5,13 +5,13 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.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;
@@ -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.
*/
@@ -129,6 +137,15 @@ public class SwerveModule
}
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);
}
/**
@@ -399,12 +416,11 @@ public class SwerveModule
angleOffset = 0;
} else
{
DriverStation.reportWarning(
"Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false);
encoderOffsetWarning.set(true);
}
} else
{
DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false);
noEncoderWarning.set(true);
}
}

23
swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
View File

@@ -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,14 @@ 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;
/**
* An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities.
*/
private Alert inaccurateVelocities;
/**
* Construct the Thrifty Encoder as a Swerve Absolute Encoder.
@@ -28,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);
}
/**
@@ -101,8 +117,7 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
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 +129,7 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true);
inaccurateVelocities.set(true);
return encoder.getValue();
}
}

64
swervelib/encoders/CANCoderSwerve.java
View File

@@ -9,7 +9,7 @@ import com.ctre.phoenix6.hardware.CANcoder;
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.
@@ -20,7 +20,23 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* CANCoder with WPILib sendable and support.
*/
public CANcoder encoder;
public CANcoder encoder;
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
private Alert magnetFieldLessThanIdeal;
/**
* An {@link Alert} for if the CANCoder reading is faulty.
*/
private Alert readingFaulty;
/**
* An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
*/
private Alert readingIgnored;
/**
* An {@link Alert} for if the absolute encoder offset cannot be set.
*/
private Alert cannotSetOffset;
/**
* Initialize the CANCoder on the standard CANBus.
@@ -41,6 +57,24 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
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);
}
/**
@@ -89,17 +123,17 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingError = false;
MagnetHealthValue strength = encoder.getMagnetHealth().getValue();
if (strength != MagnetHealthValue.Magnet_Green)
{
DriverStation.reportWarning(
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
}
magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
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<Double> angle = encoder.getAbsolutePosition().refresh();
// Taken from democat's library.
@@ -115,7 +149,10 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
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 +186,17 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
return false;
}
error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
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;
}

2
swervelib/encoders/CanAndCoderSwerve.java
View File

@@ -25,7 +25,7 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder
/**
* Reset the encoder to factory defaults.
*
* <p>
* This will not clear the stored zero offset.
*/
@Override

13
swervelib/encoders/PWMDutyCycleEncoderSwerve.java
View File

@@ -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,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;
/**
* Constructor for the PWM duty cycle encoder.
@@ -31,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);
}
/**
@@ -74,7 +83,7 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true);
inaccurateVelocities.set(true);
return encoder.get();
}

26
swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
View File

@@ -4,9 +4,9 @@ import com.revrobotics.CANSparkMax;
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.
@@ -17,7 +17,16 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
public SparkAnalogSensor encoder;
public SparkAnalogSensor encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder does not support integrated offsets.
*/
private Alert doesNotSupportIntegratedOffsets;
/**
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
@@ -34,6 +43,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
}
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
doesNotSupportIntegratedOffsets = new Alert(
"Encoders",
"SparkMax Analog Sensors do not support integrated offsets",
Alert.AlertType.WARNING_TRACE);
}
/**
@@ -50,7 +68,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
return;
}
}
DriverStation.reportWarning("Failure configuring encoder", true);
failureConfiguring.set(true);
}
/**
@@ -113,7 +131,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true);
doesNotSupportIntegratedOffsets.set(true);
return false;
}

25
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -4,9 +4,9 @@ import com.revrobotics.AbsoluteEncoder;
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.
@@ -17,7 +17,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
*/
public AbsoluteEncoder encoder;
public AbsoluteEncoder encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
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.
@@ -36,6 +44,14 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
}
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
Alert.AlertType.WARNING_TRACE);
offsetFailure = new Alert(
"Encoders",
"Failure to set Absolute Encoder Offset",
Alert.AlertType.WARNING_TRACE);
}
/**
@@ -52,7 +68,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
return;
}
}
DriverStation.reportWarning("Failure configuring encoder", true);
failureConfiguring.set(true);
}
/**
@@ -124,7 +140,8 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
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;
}

1
swervelib/encoders/SwerveAbsoluteEncoder.java
View File

@@ -56,6 +56,7 @@ public abstract class SwerveAbsoluteEncoder
/**
* Get the velocity in degrees/sec.
*
* @return velocity in degrees/sec.
*/
public abstract double getVelocity();

2
swervelib/imu/ADIS16470Swerve.java
View File

@@ -2,8 +2,8 @@ package swervelib.imu;
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;

17
swervelib/imu/NavXSwerve.java
View File

@@ -1,15 +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 java.util.Optional;
import swervelib.telemetry.Alert;
/**
* Communicates with the NavX as the IMU.
@@ -25,6 +24,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 Alert navXError;
/**
* Constructor for the NavX swerve.
@@ -33,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. */
@@ -43,7 +47,8 @@ public class NavXSwerve extends SwerveIMU
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 +69,8 @@ public class NavXSwerve extends SwerveIMU
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 +91,8 @@ public class NavXSwerve extends SwerveIMU
SmartDashboard.putData(gyro);
} catch (RuntimeException ex)
{
DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true);
navXError.setText("Error instantiating NavX: " + ex.getMessage());
navXError.set(true);
}
}

17
swervelib/imu/Pigeon2Swerve.java
View File

@@ -88,11 +88,14 @@ public class Pigeon2Swerve extends SwerveIMU
public Rotation3d getRawRotation3d()
{
// TODO: Switch to suppliers.
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> z = imu.getQuatZ();
return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue()));
StatusSignal<Double> w = imu.getQuatW();
StatusSignal<Double> x = imu.getQuatX();
StatusSignal<Double> y = imu.getQuatY();
StatusSignal<Double> 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 class Pigeon2Swerve extends SwerveIMU
StatusSignal<Double> yAcc = imu.getAccelerationX();
StatusSignal<Double> 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));
}
/**

2
swervelib/math/SwerveMath.java
View File

@@ -13,8 +13,6 @@ import swervelib.SwerveController;
import swervelib.SwerveModule;
import swervelib.parser.SwerveDriveConfiguration;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.telemetry.SwerveDriveTelemetry;
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* Mathematical functions which pertain to swerve drive.

58
swervelib/motors/SparkFlexSwerve.java
View File

@@ -1,21 +1,21 @@
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 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;
/**
@@ -27,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)
*/
@@ -39,11 +39,19 @@ 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;
/**
* An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
*/
private Alert absoluteEncoderOffsetWarning;
/**
* Initialize the swerve motor.
@@ -65,6 +73,15 @@ public class SparkFlexSwerve extends SwerveMotor
// 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);
}
/**
@@ -92,7 +109,7 @@ public class SparkFlexSwerve extends SwerveMotor
return;
}
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
failureConfiguring.set(true);
}
/**
@@ -185,10 +202,7 @@ public class SparkFlexSwerve extends SwerveMotor
{
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()));
}
@@ -357,20 +371,20 @@ public class SparkFlexSwerve extends SwerveMotor
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);
}

38
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -1,20 +1,20 @@
package swervelib.motors;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
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 edu.wpi.first.wpilibj.DriverStation;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.Alert;
/**
* Brushed motor control with SparkMax.
@@ -30,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.
*/
@@ -42,7 +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;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguringAlert;
/**
* An {@link Alert} for if the motor has no encoder defined.
*/
private Alert noEncoderDefinedAlert;
/**
* Initialize the swerve motor.
@@ -59,7 +71,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
// 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.");
}
@@ -90,6 +102,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
}
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback.
noEncoderAlert = new Alert("Motors",
"Cannot use motor without encoder.",
Alert.AlertType.ERROR_TRACE);
failureConfiguringAlert = new Alert("Motors",
"Failure configuring motor " + motor.getDeviceId(),
Alert.AlertType.WARNING_TRACE);
noEncoderDefinedAlert = new Alert("Motors",
"An encoder MUST be defined to work with a SparkMAX",
Alert.AlertType.ERROR_TRACE);
}
/**
@@ -122,7 +144,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
return;
}
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
failureConfiguringAlert.set(true);
}
/**
@@ -220,7 +242,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
}
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;

22
swervelib/motors/TalonFXSwerve.java
View File

@@ -20,31 +20,31 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Factory default already occurred.
*/
private final boolean factoryDefaultOccurred = false;
/**
* Current TalonFX configuration.
*/
private TalonFXConfiguration configuration = new TalonFXConfiguration();
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);
// /**
// * 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);
/**
* TalonFX motor controller.
*/
TalonFX motor;
/**
* Current TalonFX configuration.
*/
private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
* Constructor for TalonFX swerve motor.

19
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -89,7 +89,22 @@ public class SwerveDriveConfiguration
*/
public double getDriveBaseRadiusMeters()
{
Translation2d furthestModule = moduleLocationsMeters[0];
return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2)));
//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]);
//Find Largest Radius by checking the distance to the center point
double largestRadius = centerOfModules.getDistance(moduleLocationsMeters[0]);
for (int i = 1; i < moduleLocationsMeters.length; i++)
{
if (largestRadius < centerOfModules.getDistance(moduleLocationsMeters[i]))
{
largestRadius = centerOfModules.getDistance(moduleLocationsMeters[i]);
}
}
//Return Largest Radius
return largestRadius;
}
}

29
swervelib/parser/json/DeviceJson.java
View File

@@ -26,6 +26,7 @@ import swervelib.motors.SparkMaxSwerve;
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.
@@ -33,18 +34,30 @@ import swervelib.motors.TalonSRXSwerve;
public class DeviceJson
{
/**
* An {@link Alert} for if the CAN ID is greater than 40.
*/
private final Alert canIdWarning = new Alert("JSON",
"CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
Alert.AlertType.WARNING);
/**
* An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
*/
private final Alert i2cLockupWarning = new Alert("IMU",
"I2C lockup issue detected on roboRIO. Check console for more information.",
Alert.AlertType.WARNING);
/**
* The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx
*/
public String type;
public String type;
/**
* The CAN ID or pin ID of the device.
*/
public int id;
public int id;
/**
* The CAN bus name which the device resides on if using CAN.
*/
public String canbus = "";
public String canbus = "";
/**
* Create a {@link SwerveAbsoluteEncoder} from the current configuration.
@@ -57,8 +70,7 @@ public class DeviceJson
{
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 +111,7 @@ public class DeviceJson
{
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 +132,7 @@ public class DeviceJson
"\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 +157,7 @@ public class DeviceJson
{
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)
{

213
swervelib/telemetry/Alert.java Normal file
View File

@@ -0,0 +1,213 @@
// 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<String, SendableAlerts> groups = new HashMap<String, SendableAlerts>();
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
{
/**
* 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
}
private static class SendableAlerts implements Sendable
{
public final List<Alert> alerts = new ArrayList<>();
public String[] getStrings(AlertType type)
{
Predicate<Alert> activeFilter = (Alert x) -> x.type == type && x.active;
Comparator<Alert> 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);
}
}
}