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

Updated Javadocs, added ability to push offset to absolute encoders.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-12-12 10:48:54 -06:00
parent aef91407ea
commit f03926627d
123 changed files with 685 additions and 283 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
swervelib/SwerveDrive.java
View File

@@ -66,11 +66,11 @@ public class SwerveDrive
/**
* Odometry lock to ensure thread safety.
*/
private final Lock odometryLock = new ReentrantLock();
private final Lock odometryLock = new ReentrantLock();
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
@@ -79,31 +79,31 @@ public class SwerveDrive
* Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of
* rotation)
*/
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
public Matrix<N3, N1> stateStdDevs = VecBuilder.fill(0.1,
0.1,
0.1);
/**
* The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more
* points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)}
* with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get
* vision accurate to inches instead of feet.
*/
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
public Matrix<N3, N1> visionMeasurementStdDevs = VecBuilder.fill(0.9,
0.9,
0.9);
/**
* Invert odometry readings of drive motor positions, used as a patch for debugging currently.
*/
public boolean invertOdometry = false;
public boolean invertOdometry = false;
/**
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
public boolean chassisVelocityCorrection = true;
public boolean chassisVelocityCorrection = true;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
public boolean headingCorrection = false;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
@@ -115,23 +115,23 @@ public class SwerveDrive
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
private double lastHeadingRadians = 0;
/**
* The absolute max speed that your robot can reach while translating in meters per second.
*/
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
/**
* The absolute max speed the robot can reach while rotating radians per second.
*/
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
/**
* Maximum speed of the robot in meters per second.
*/
private double maxSpeedMPS;
private double maxSpeedMPS;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -902,7 +902,8 @@ public class SwerveDrive
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition());
SmartDashboard.putNumber(
"Module[" + module.configuration.name + "] Absolute Encoder Read Issue", module.getAbsoluteEncoderReadIssue() ? 1 : 0);
"Module[" + module.configuration.name + "] Absolute Encoder Read Issue",
module.getAbsoluteEncoderReadIssue() ? 1 : 0);
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
{
@@ -1071,4 +1072,27 @@ public class SwerveDrive
}
}
/**
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.
*/
public void pushOffsetsToControllers()
{
for (SwerveModule module : swerveModules)
{
module.pushOffsetsToControllers();
}
}
/**
* Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0
*/
public void restoreInternalOffset()
{
for (SwerveModule module : swerveModules)
{
module.restoreInternalOffset();
}
}
}

60
swervelib/SwerveModule.java
View File

@@ -4,6 +4,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
@@ -23,10 +24,6 @@ public class SwerveModule
* Swerve module configuration options.
*/
public final SwerveModuleConfiguration configuration;
/**
* Angle offset from the absolute encoder.
*/
private final double angleOffset;
/**
* Swerve Motors.
*/
@@ -38,32 +35,36 @@ public class SwerveModule
/**
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
*/
public int moduleNumber;
public int moduleNumber;
/**
* Feedforward for drive motor during closed loop control.
*/
public SimpleMotorFeedforward feedforward;
public SimpleMotorFeedforward feedforward;
/**
* Maximum speed of the drive motors in meters per second.
*/
public double maxSpeed;
public double maxSpeed;
/**
* Last swerve module state applied.
*/
public SwerveModuleState lastState;
public SwerveModuleState lastState;
/**
* Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module
* never turns more than 90deg.
*/
public boolean moduleStateOptimization = true;
public boolean moduleStateOptimization = true;
/**
* Angle offset from the absolute encoder.
*/
private double angleOffset;
/**
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
private SwerveModuleSimulation simModule;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
@@ -388,6 +389,36 @@ public class SwerveModule
return configuration;
}
/**
* Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
*/
public void pushOffsetsToControllers()
{
if (absoluteEncoder != null)
{
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
{
angleOffset = 0;
} else
{
DriverStation.reportWarning(
"Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false);
}
} else
{
DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false);
}
}
/**
* Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value.
*/
public void restoreInternalOffset()
{
absoluteEncoder.setAbsoluteEncoderOffset(0);
angleOffset = configuration.angleOffset;
}
/**
* Get if the last Absolute Encoder had a read issue, such as it does not exist.
*
@@ -395,9 +426,12 @@ public class SwerveModule
*/
public boolean getAbsoluteEncoderReadIssue()
{
if(absoluteEncoder == null)
if (absoluteEncoder == null)
{
return true;
else
} else
{
return absoluteEncoder.readingError;
}
}
}

17
swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
View File

@@ -91,6 +91,21 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
return encoder;
}
/**
* Cannot Set the offset of an Analog Absolute Encoder.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return Will always be false as setting the offset is unsupported of an Analog absolute encoder.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
//Do Nothing
DriverStation.reportWarning(
"Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false);
return false;
}
/**
* Get the velocity in degrees/sec.
*
@@ -99,7 +114,7 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!",true);
DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true);
return encoder.getValue();
}
}

19
swervelib/encoders/CANCoderSwerve.java
View File

@@ -138,6 +138,25 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
return encoder;
}
/**
* Sets the Absolute Encoder Offset inside of the CANcoder's Memory.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
ErrorCode error = encoder.configMagnetOffset(offset);
if (error == ErrorCode.OK)
{
return true;
}
DriverStation.reportWarning(
"Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false);
return false;
}
/**
* Get the velocity in degrees/sec.
*

16
swervelib/encoders/CanAndCoderSwerve.java
View File

@@ -1,6 +1,7 @@
package swervelib.encoders;
import com.reduxrobotics.sensors.canandcoder.CANandcoder;
import edu.wpi.first.wpilibj.DriverStation;
/**
* HELIUM {@link CANandcoder} from ReduxRobotics absolute encoder, attached through the CAN bus.
@@ -78,6 +79,21 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder
return encoder;
}
/**
* Cannot set the offset of the CanAndCoder.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return always false due to CanAndCoder not supporting offset changing.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
//CanAndCoder does not support Absolute Offset Changing
DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(),
false);
return false;
}
/**
* Get the velocity in degrees/sec.
*

13
swervelib/encoders/PWMDutyCycleEncoderSwerve.java
View File

@@ -96,4 +96,17 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
// Do nothing
}
/**
* Sets the offset of the Encoder in the WPILib Encoder Library.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return Always true due to no external device commands.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
encoder.setPositionOffset(offset);
return true;
}
}

21
swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
View File

@@ -1,6 +1,5 @@
package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.SparkMaxAnalogSensor;
@@ -16,14 +15,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax.
* The {@link SparkMaxAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port.
*/
public SparkMaxAnalogSensor encoder;
/**
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data port analog pin.
* Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data
* port analog pin.
*
* @param motor Motor to create the encoder from.
* @param motor Motor to create the encoder from.
*/
public SparkMaxAnalogEncoderSwerve(SwerveMotor motor)
{
@@ -104,6 +104,19 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
return encoder;
}
/**
* Sets the Absolute Encoder offset at the Encoder Level.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true);
return false;
}
/**
* Get the velocity in degrees/sec.
*

22
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -106,6 +106,28 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
return encoder;
}
/**
* Sets the Absolute Encoder Offset inside of the SparkMax's Memory.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
REVLibError error = null;
for (int i = 0; i < maximumRetries; i++)
{
error = encoder.setZeroOffset(offset);
if (error == REVLibError.kOk)
{
return true;
}
}
DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false);
return false;
}
/**
* Get the velocity in degrees/sec.
*

8
swervelib/encoders/SwerveAbsoluteEncoder.java
View File

@@ -46,6 +46,14 @@ public abstract class SwerveAbsoluteEncoder
*/
public abstract Object getAbsoluteEncoder();
/**
* Sets the Absolute Encoder offset at the Encoder Level.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return if setting Absolute Encoder Offset was successful or not.
*/
public abstract boolean setAbsoluteEncoderOffset(double offset);
/**
* Get the velocity in degrees/sec.
* @return velocity in degrees/sec.

12
swervelib/motors/SparkMaxSwerve.java
View File

@@ -214,19 +214,23 @@ public class SparkMaxSwerve extends SwerveMotor
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(positionConversionFactor);
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
} else
{
return ((SparkMaxAnalogSensor)absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(positionConversionFactor);
return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(
positionConversionFactor);
}
});
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(positionConversionFactor / 60);
return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
} else
{
return ((SparkMaxAnalogSensor)absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(positionConversionFactor / 60);
return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(
positionConversionFactor / 60);
}
});
}

2
swervelib/parser/SwerveModulePhysicalCharacteristics.java
View File

@@ -11,7 +11,7 @@ public class SwerveModulePhysicalCharacteristics
/**
* Current limits for the Swerve Module.
*/
public final int driveMotorCurrentLimit, angleMotorCurrentLimit;
public final int driveMotorCurrentLimit, angleMotorCurrentLimit;
/**
* The time it takes for the motor to go from 0 to full throttle in seconds.
*/

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

@@ -130,7 +130,7 @@ public class DeviceJson
case "pigeon2":
return new Pigeon2Swerve(id, canbus != null ? canbus : "");
default:
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
throw new RuntimeException(type + " is not a recognized imu/gyroscope type.");
}
}
@@ -181,7 +181,7 @@ public class DeviceJson
case "talonsrx":
return new TalonSRXSwerve(id, isDriveMotor);
default:
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
throw new RuntimeException(type + " is not a recognized motor type.");
}
}
}

2
swervelib/parser/json/PhysicalPropertiesJson.java
View File

@@ -14,7 +14,7 @@ public class PhysicalPropertiesJson
* {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or
* {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors.
*/
public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0);
/**
* The current limit in AMPs to apply to the motors.
*/