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 to 2024.4.6.3

Browse Source
This commit is contained in:
thenetworkgrinch
2024-01-26 11:29:15 -06:00
parent bd949c2fa5
commit 75ec1aee24
121 changed files with 1751 additions and 552 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
swervelib/SwerveDrive.java
View File

@@ -98,10 +98,6 @@ public class SwerveDrive
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Whether heading correction PID is currently active.
*/
private boolean correctionEnabled = false;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
@@ -177,6 +173,7 @@ public class SwerveDrive
Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
setMaximumSpeed(maxSpeedMPS);
// Initialize Telemetry
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
@@ -452,16 +449,11 @@ public class SwerveDrive
&& (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND
|| Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND))
{
if (!correctionEnabled)
{
lastHeadingRadians = getOdometryHeading().getRadians();
correctionEnabled = true;
}
velocity.omegaRadiansPerSecond =
swerveController.headingCalculate(lastHeadingRadians, getOdometryHeading().getRadians());
swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians);
} else
{
correctionEnabled = false;
lastHeadingRadians = getOdometryHeading().getRadians();
}
}
@@ -686,6 +678,33 @@ public class SwerveDrive
return positions;
}
/**
* Getter for the {@link SwerveIMU}.
*
* @return generated {@link SwerveIMU}
*/
public SwerveIMU getGyro()
{
return swerveDriveConfiguration.imu;
}
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
* subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
* @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
if (SwerveDriveTelemetry.isSimulation)
{
setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
} else
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
}
/**
* Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0.
*/
@@ -820,6 +839,7 @@ public class SwerveDrive
swerveDriveConfiguration.physicalCharacteristics.optimalVoltage = optimalVoltage;
for (SwerveModule module : swerveModules)
{
module.maxSpeed = maximumSpeed;
if (updateModuleFeedforward)
{
module.feedforward = SwerveMath.createDriveFeedforward(optimalVoltage,
@@ -1030,9 +1050,7 @@ public class SwerveDrive
/**
* Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with
* the given timestamp of the vision measurement. <br /> <b>IT IS HIGHLY RECOMMENDED TO UPDATE YOUR GYROSCOPE OFFSET
* AFTER USING THIS FUNCTION!</b> <br /> To update your gyroscope readings directly use
* {@link SwerveDrive#setGyroOffset(Rotation3d)}.
* the given timestamp of the vision measurement.
*
* @param robotPose Robot {@link Pose2d} as measured by vision.
* @param timestamp Timestamp the measurement was taken as time since startup, should be taken from
@@ -1050,24 +1068,6 @@ public class SwerveDrive
// resetOdometry(newOdometry);
}
/**
* Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d}
* subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}.
*
* @param gyro expected gyroscope angle as {@link Rotation3d}.
*/
public void setGyro(Rotation3d gyro)
{
if (SwerveDriveTelemetry.isSimulation)
{
setGyroOffset(simIMU.getGyroRotation3d().minus(gyro));
} else
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
}
/**
* Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to
* generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply

198
swervelib/SwerveDriveTest.java Normal file
View File

@@ -0,0 +1,198 @@
package swervelib;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import swervelib.encoders.SwerveAbsoluteEncoder;
/**
* Class to perform tests on the swerve drive.
*/
public class SwerveDriveTest
{
/**
* Set the angle of the modules to a given {@link Rotation2d}
*
* @param swerveDrive {@link SwerveDrive} to use.
* @param moduleAngle {@link Rotation2d} to set every module to.
*/
public static void angleModules(SwerveDrive swerveDrive, Rotation2d moduleAngle)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
swerveModule.setDesiredState(new SwerveModuleState(0, moduleAngle), false, true);
}
}
/**
* Power the drive motors for the swerve drive to a set duty cycle percentage.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param percentage Duty cycle percentage of voltage to send to drive motors.
*/
public static void powerDriveMotorsDutyCycle(SwerveDrive swerveDrive, double percentage)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
swerveModule.getDriveMotor().set(percentage);
}
}
/**
* Power the angle motors for the swerve drive to a set percentage.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param percentage DutyCycle percentage to send to angle motors.
*/
public static void powerAngleMotors(SwerveDrive swerveDrive, double percentage)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
swerveModule.getAngleMotor().set(percentage);
}
}
/**
* Power the drive motors for the swerve drive to a set voltage.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param volts DutyCycle percentage of voltage to send to drive motors.
*/
public static void powerDriveMotorsVoltage(SwerveDrive swerveDrive, double volts)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
swerveModule.getDriveMotor().setVoltage(volts);
}
}
/**
* Power the angle motors for the swerve drive to a set voltage.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param volts Voltage to send to angle motors.
*/
public static void powerAngleMotorsVoltage(SwerveDrive swerveDrive, double volts)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
swerveModule.getAngleMotor().setVoltage(volts);
}
}
/**
* Set the modules to center to 0.
*
* @param swerveDrive Swerve Drive to control.
*/
public static void centerModules(SwerveDrive swerveDrive)
{
angleModules(swerveDrive, Rotation2d.fromDegrees(0));
}
/**
* Find the minimum amount of power required to move the swerve drive motors.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param minMovement Minimum amount of movement to drive motors.
* @param testDelaySeconds Time in seconds for the motor to move.
* @param maxVolts The maximum voltage to send to drive motors.
* @return minimum voltage required.
*/
public static double findDriveMotorKV(SwerveDrive swerveDrive, double minMovement, double testDelaySeconds,
double maxVolts)
{
double[] startingEncoders = new double[4];
double kV = 0;
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
SwerveModule[] modules = swerveDrive.getModules();
for (int i = 0; i < modules.length; i++)
{
startingEncoders[i] = Math.abs(modules[i].getDriveMotor().getPosition());
}
for (double kV_new = 0; kV_new < maxVolts; kV_new += 0.0001)
{
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, kV);
boolean foundkV = false;
double startTimeSeconds = Timer.getFPGATimestamp();
while ((Timer.getFPGATimestamp() - startTimeSeconds) < testDelaySeconds && !foundkV)
{
for (int i = 0; i < modules.length; i++)
{
if ((modules[i].getDriveMotor().getPosition() - startingEncoders[i]) > minMovement)
{
foundkV = true;
break;
}
}
}
if (foundkV)
{
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
kV = kV_new;
}
}
return kV;
}
/**
* Find the coupling ratio for all modules.
*
* @param swerveDrive {@link SwerveDrive} to operate with.
* @param volts Voltage to send to angle motors to spin.
* @param automatic Attempt to automatically spin the modules.
* @return Average coupling ratio.
*/
public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, boolean automatic)
{
System.out.println("Stopping the Swerve Drive.");
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, 0);
Timer.delay(1);
double couplingRatioSum = 0;
for (SwerveModule module : swerveDrive.getModules())
{
if (module.getAbsoluteEncoder() == null)
{
throw new RuntimeException("Absolute encoders are required to find the coupling ratio.");
}
SwerveAbsoluteEncoder absoluteEncoder = module.getAbsoluteEncoder();
if (absoluteEncoder.readingError)
{
throw new RuntimeException("Absolute encoder encountered a reading error please debug.");
}
System.out.println("Fetching the current absolute encoder and drive encoder position.");
module.getAngleMotor().setVoltage(0);
Timer.delay(1);
Rotation2d startingAbsoluteEncoderPosition = Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition());
double driveEncoderPositionRotations = module.getDriveMotor().getPosition() /
module.configuration.conversionFactors.drive;
if (automatic)
{
module.getAngleMotor().setVoltage(volts);
Timer.delay(0.01);
System.out.println("Rotating the module 360 degrees");
while (!Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition()).equals(startingAbsoluteEncoderPosition))
;
module.getAngleMotor().setVoltage(0);
} else
{
DriverStation.reportWarning(
"Spin the " + module.configuration.name + " module 360 degrees now, you have 1 minute.\n",
false);
Timer.delay(60);
}
double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive) -
driveEncoderPositionRotations;
DriverStation.reportWarning(module.configuration.name + " Coupling Ratio: " + couplingRatio, false);
couplingRatioSum += couplingRatio;
}
DriverStation.reportWarning("Average Coupling Ratio: " + (couplingRatioSum / 4.0), false);
return (couplingRatioSum / 4.0);
}
}

33
swervelib/SwerveModule.java
View File

@@ -112,7 +112,6 @@ public class SwerveModule
{
absoluteEncoder.factoryDefault();
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
angleMotor.setPosition(getAbsolutePosition());
}
// Config angle motor/controller
@@ -122,6 +121,12 @@ public class SwerveModule
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
angleMotor.setMotorBrake(false);
// Set the position AFTER settings the conversion factor.
if (absoluteEncoder != null)
{
angleMotor.setPosition(getAbsolutePosition());
}
// Config drive motor/controller
driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive);
driveMotor.configurePIDF(moduleConfiguration.velocityPIDF);
@@ -199,18 +204,22 @@ public class SwerveModule
driveMotor.set(percentOutput);
} else
{
// Taken from the CTRE SwerveModule class.
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
/* To reduce the "skew" that occurs when changing direction */
double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
double cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0 || desiredState.speedMetersPerSecond == 0)
double cosineScalar = 1.0;
if (configuration.useCosineCompensator)
{
cosineScalar = 0.0;
// Taken from the CTRE SwerveModule class.
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
/* To reduce the "skew" that occurs when changing direction */
double steerMotorError = desiredState.angle.getDegrees() - getAbsolutePosition();
/* If error is close to 0 rotations, we're already there, so apply full power */
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
cosineScalar = Math.cos(Units.degreesToRadians(steerMotorError));
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
if (cosineScalar < 0.0)
{
cosineScalar = 0.0;
}
}
double velocity = desiredState.speedMetersPerSecond * (cosineScalar);

20
swervelib/imu/Pigeon2Swerve.java
View File

@@ -102,14 +102,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();
Rotation3d reading = 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();
Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(),
x.refresh().getValue(),
y.refresh().getValue(),
z.refresh().getValue()));
return invertedIMU ? reading.unaryMinus() : reading;
}
@@ -135,8 +135,8 @@ public class Pigeon2Swerve extends SwerveIMU
{
// TODO: Switch to suppliers.
StatusSignal<Double> xAcc = imu.getAccelerationX();
StatusSignal<Double> yAcc = imu.getAccelerationX();
StatusSignal<Double> zAcc = imu.getAccelerationX();
StatusSignal<Double> yAcc = imu.getAccelerationY();
StatusSignal<Double> zAcc = imu.getAccelerationZ();
return Optional.of(new Translation3d(xAcc.refresh().getValue(),
yAcc.refresh().getValue(),

33
swervelib/motors/SparkFlexSwerve.java
View File

@@ -404,6 +404,39 @@ public class SparkFlexSwerve extends SwerveMotor
setReference(setpoint, feedforward);
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getAppliedOutput() * motor.getBusVoltage();
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getAppliedOutput();
}
/**
* Get the velocity of the integrated encoder.
*

33
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -402,6 +402,39 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
setReference(setpoint, feedforward);
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getAppliedOutput() * motor.getBusVoltage();
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getAppliedOutput();
}
/**
* Get the velocity of the integrated encoder.
*

33
swervelib/motors/SparkMaxSwerve.java
View File

@@ -392,6 +392,39 @@ public class SparkMaxSwerve extends SwerveMotor
setReference(setpoint, feedforward);
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getAppliedOutput() * motor.getBusVoltage();
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getAppliedOutput();
}
/**
* Get the velocity of the integrated encoder.
*

21
swervelib/motors/SwerveMotor.java
View File

@@ -101,6 +101,27 @@ public abstract class SwerveMotor
*/
public abstract void setReference(double setpoint, double feedforward, double position);
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
public abstract double getVoltage();
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
public abstract void setVoltage(double voltage);
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
public abstract double getAppliedOutput();
/**
* Get the velocity of the integrated encoder.
*

35
swervelib/motors/TalonFXSwerve.java
View File

@@ -40,7 +40,7 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Conversion factor for the motor.
*/
private double conversionFactor;
private double conversionFactor;
/**
* Current TalonFX configuration.
*/
@@ -343,6 +343,39 @@ public class TalonFXSwerve extends SwerveMotor
}
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getMotorVoltage().refresh().getValue();
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getDutyCycle().refresh().getValue();
}
/**
* Get the velocity of the integrated encoder.
*

34
swervelib/motors/TalonSRXSwerve.java
View File

@@ -256,7 +256,6 @@ public class TalonSRXSwerve extends SwerveMotor
motor.set(percentOutput);
}
/**
* Convert the setpoint into native sensor units.
*
@@ -303,6 +302,39 @@ public class TalonSRXSwerve extends SwerveMotor
feedforward / nominalVoltage);
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getMotorOutputVoltage();
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getMotorOutputPercent();
}
/**
* Get the velocity of the integrated encoder.
*

16
swervelib/parser/SwerveModuleConfiguration.java
View File

@@ -62,6 +62,10 @@ public class SwerveModuleConfiguration
* Name for the swerve module for telemetry.
*/
public String name;
/**
* Should do cosine compensation when not pointing correct direction;.
*/
public boolean useCosineCompensator;
/**
* Construct a configuration object for swerve modules.
@@ -80,6 +84,7 @@ public class SwerveModuleConfiguration
* @param physicalCharacteristics Physical characteristics of the swerve module.
* @param name The name for the swerve module.
* @param conversionFactors Conversion factors to be applied to the drive and angle motors.
* @param useCosineCompensator Should use cosineCompensation.
*/
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
@@ -95,7 +100,8 @@ public class SwerveModuleConfiguration
boolean absoluteEncoderInverted,
boolean driveMotorInverted,
boolean angleMotorInverted,
String name)
String name,
boolean useCosineCompensator)
{
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
@@ -110,6 +116,7 @@ public class SwerveModuleConfiguration
this.velocityPIDF = velocityPIDF;
this.physicalCharacteristics = physicalCharacteristics;
this.name = name;
this.useCosineCompensator = useCosineCompensator;
}
/**
@@ -127,6 +134,7 @@ public class SwerveModuleConfiguration
* @param velocityPIDF Velocity PIDF configuration.
* @param physicalCharacteristics Physical characteristics of the swerve module.
* @param name Name for the module.
* @param useCosineCompensator Should use cosineCompensation.
*/
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
@@ -139,7 +147,8 @@ public class SwerveModuleConfiguration
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
SwerveModulePhysicalCharacteristics physicalCharacteristics,
String name)
String name,
boolean useCosineCompensator)
{
this(
driveMotor,
@@ -155,7 +164,8 @@ public class SwerveModuleConfiguration
false,
false,
false,
name);
name,
useCosineCompensator);
}

7
swervelib/parser/json/ModuleJson.java
View File

@@ -53,6 +53,10 @@ public class ModuleJson
* The location of the swerve module from the center of the robot in inches.
*/
public LocationJson location;
/**
* Should do cosine compensation when not pointing correct direction;.
*/
public boolean useCosineCompensator = true;
/**
* Create the swerve module configuration based off of parsed data.
@@ -131,6 +135,7 @@ public class ModuleJson
absoluteEncoderInverted,
inverted.drive,
inverted.angle,
name.replaceAll("\\.json", ""));
name.replaceAll("\\.json", ""),
useCosineCompensator);
}
}