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

Upgrading to 2025.1.2

Browse Source
This commit is contained in:
thenetworkgrinch
2025-01-06 15:44:15 +00:00
parent 8050f43fa5
commit 62f8236678
27 changed files with 1083 additions and 305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
swervelib/SwerveDrive.java
View File

@@ -52,11 +52,14 @@ import java.util.Map;
import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import org.ironmaple.simulation.SimulatedArena;
import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
@@ -243,7 +246,7 @@ public class SwerveDrive
.withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg))
.withCustomModuleTranslations(config.moduleLocationsMeters)
.withGyro(config.getGyroSim())
.withSwerveModule(() -> new SwerveModuleSimulation(
.withSwerveModule(new SwerveModuleSimulationConfig(
config.getDriveMotorSim(),
config.getAngleMotorSim(),
config.physicalCharacteristics.conversionFactor.drive.gearRatio,
@@ -254,7 +257,8 @@ public class SwerveDrive
config.physicalCharacteristics.conversionFactor.drive.diameter /
2),
KilogramSquareMeters.of(0.02),
config.physicalCharacteristics.wheelGripCoefficientOfFriction));
config.physicalCharacteristics.wheelGripCoefficientOfFriction)
);
mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
@@ -452,39 +456,38 @@ public class SwerveDrive
/**
* Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same
* time. The inputs are added together so this is not intneded to be used to give the driver both methods of control.
* time. The inputs are added together so this is not intended to be used to give the driver both methods of control.
*
* @param fieldOrientedVelocity The field oriented velocties to use
* @param robotOrientedVelocity The robot oriented velocties to use
*/
public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity,
public void driveFieldOrientedAndRobotOriented(ChassisSpeeds fieldOrientedVelocity,
ChassisSpeeds robotOrientedVelocity)
{
fieldOrientedVelocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(fieldOrientedVelocity.plus(robotOrientedVelocity));
drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading())
.plus(robotOrientedVelocity));
}
/**
* Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
*
* @param velocity Velocity of the robot desired.
* @param fieldRelativeSpeeds Velocity of the robot desired.
*/
public void driveFieldOriented(ChassisSpeeds velocity)
public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(velocity);
drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading()));
}
/**
* Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
*
* @param velocity Velocity of the robot desired.
* @param fieldRelativeSpeeds Velocity of the robot desired.
* @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot.
*/
public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters)
public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds, Translation2d centerOfRotationMeters)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
drive(velocity, centerOfRotationMeters);
drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading()), centerOfRotationMeters);
}
/**
@@ -535,7 +538,7 @@ public class SwerveDrive
ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
if (fieldRelative)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
}
drive(velocity, isOpenLoop, centerOfRotationMeters);
}
@@ -564,7 +567,7 @@ public class SwerveDrive
if (fieldRelative)
{
velocity.toRobotRelativeSpeeds(getOdometryHeading());
ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
}
drive(velocity, isOpenLoop, new Translation2d());
}
@@ -730,7 +733,6 @@ public class SwerveDrive
/**
* Drive the robot using the {@link SwerveModuleState}, it is recommended to have
* {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this.<br/>
* <p>
*
* @param robotRelativeVelocity Robot relative {@link ChassisSpeeds}
* @param states Corresponding {@link SwerveModuleState} to use (not checked against the
@@ -844,8 +846,10 @@ public class SwerveDrive
// but not the reverse. However, because this transform is a simple rotation, negating the
// angle given as the robot angle reverses the direction of rotation, and the conversion is reversed.
ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates());
robotRelativeSpeeds.toFieldRelativeSpeeds(getOdometryHeading());//.unaryMinus());
return robotRelativeSpeeds;
return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeSpeeds, getOdometryHeading());
// Might need to be this instead
//return ChassisSpeeds.fromFieldRelativeSpeeds(
// kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
}
/**
@@ -874,8 +878,7 @@ public class SwerveDrive
mapleSimDrive.setSimulationWorldPose(pose);
}
odometryLock.unlock();
ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds();
robotRelativeSpeeds.toFieldRelativeSpeeds(getYaw());
ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(new ChassisSpeeds(0, 0, 0), getYaw());
kinematics.toSwerveModuleStates(robotRelativeSpeeds);
}
@@ -1408,8 +1411,8 @@ public class SwerveDrive
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
*
* @param enable Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following.
* @param enable Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)}} with the following.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
@@ -1425,10 +1428,10 @@ public class SwerveDrive
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
* and/or auto
*
* @param useInTeleop Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with
* the following in auto.
* @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
@@ -1476,8 +1479,10 @@ public class SwerveDrive
var angularVelocity = new Rotation2d(imu.getYawAngularVelocity().in(RadiansPerSecond) * angularVelocityCoefficient);
if (angularVelocity.getRadians() != 0.0)
{
robotRelativeVelocity.toFieldRelativeSpeeds(getOdometryHeading());
robotRelativeVelocity.toRobotRelativeSpeeds(getOdometryHeading().plus(angularVelocity));
ChassisSpeeds fieldRelativeVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity,
getOdometryHeading());
robotRelativeVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeVelocity,
getOdometryHeading().plus(angularVelocity));
}
return robotRelativeVelocity;
}
@@ -1503,7 +1508,7 @@ public class SwerveDrive
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (uesChassisDiscretize)
{
robotRelativeVelocity.discretize(discretizationdtSeconds);
robotRelativeVelocity = ChassisSpeeds.discretize(robotRelativeVelocity, discretizationdtSeconds);
}
return robotRelativeVelocity;

369
swervelib/SwerveInputStream.java
View File

@@ -6,6 +6,8 @@ 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.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.XboxController;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
@@ -13,11 +15,28 @@ import java.util.function.Supplier;
import swervelib.math.SwerveMath;
/**
* Helper class to easily transform Controller inputs into workable Chassis speeds. <br /> Inspired by SciBorgs.
* https://github.com/SciBorgs/Crescendo-2024/blob/main/src/main/java/org/sciborgs1155/lib/InputStream.java
* Helper class to easily transform Controller inputs into workable Chassis speeds. Intended to easily create an
* interface that generates {@link ChassisSpeeds} from {@link XboxController}
* <p>
* Intended to easily create an interface that generates {@link ChassisSpeeds} from
* {@link edu.wpi.first.wpilibj.XboxController}
* <br /> Inspired by SciBorgs FRC 1155. <br /> Example:
* <pre>
* {@code
* XboxController driverXbox = new XboxController(0);
*
* SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
* () -> driverXbox.getLeftY() * -1,
* () -> driverXbox.getLeftX() * -1) // Axis which give the desired translational angle and speed.
* .withControllerRotationAxis(driverXbox::getRightX) // Axis which give the desired angular velocity.
* .deadband(0.01) // Controller deadband
* .scaleTranslation(0.8) // Scaled controller translation axis
* .allianceRelativeControl(true); // Alliance relative controls.
*
* SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() // Copy the stream so further changes do not affect driveAngularVelocity
* .withControllerHeadingAxis(driverXbox::getRightX,
* driverXbox::getRightY) // Axis which give the desired heading angle using trigonometry.
* .headingWhile(true); // Enable heading based control.
* }
* </pre>
*/
public class SwerveInputStream implements Supplier<ChassisSpeeds>
{
@@ -37,7 +56,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/**
* Rotation supplier as angular velocity.
*/
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
/**
* Controller supplier as heading.
*/
@@ -49,19 +68,19 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/**
* Axis deadband for the controller.
*/
private Optional<Double> axisDeadband = Optional.empty();
private Optional<Double> axisDeadband = Optional.empty();
/**
* Translational axis scalar value, should be between (0, 1].
*/
private Optional<Double> translationAxisScale = Optional.empty();
private Optional<Double> translationAxisScale = Optional.empty();
/**
* Angular velocity axis scalar value, should be between (0, 1]
*/
private Optional<Double> omegaAxisScale = Optional.empty();
private Optional<Double> omegaAxisScale = Optional.empty();
/**
* Target to aim at.
*/
private Optional<Pose2d> aimTarget = Optional.empty();
private Optional<Pose2d> aimTarget = Optional.empty();
/**
* Output {@link ChassisSpeeds} based on heading while this is True.
*/
@@ -78,6 +97,22 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
* Maintain current heading and drive without rotating, ideally.
*/
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
/**
* Cube the translation magnitude from the controller.
*/
private Optional<BooleanSupplier> translationCube = Optional.empty();
/**
* Cube the angular velocity axis from the controller.
*/
private Optional<BooleanSupplier> omegaCube = Optional.empty();
/**
* Robot relative oriented output expected.
*/
private Optional<BooleanSupplier> robotRelative = Optional.empty();
/**
* Field oriented chassis output is relative to your current alliance.
*/
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
/**
* {@link SwerveController} for simple control over heading.
*/
@@ -88,53 +123,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY;
/**
* Drive modes to keep track of.
*/
enum SwerveInputMode
{
/**
* Translation only mode, does not allow for rotation and maintains current heading.
*/
TRANSLATION_ONLY,
/**
* Output based off angular velocity
*/
ANGULAR_VELOCITY,
/**
* Output based off of heading.
*/
HEADING,
/**
* Output based off of targeting.
*/
AIM
}
/**
* Copy the {@link SwerveInputStream} object.
*
* @return Clone of current {@link SwerveInputStream}
*/
public SwerveInputStream copy()
{
SwerveInputStream newStream = new SwerveInputStream(swerveDrive, controllerTranslationX, controllerTranslationY);
newStream.controllerOmega = controllerOmega;
newStream.controllerHeadingX = controllerHeadingX;
newStream.controllerHeadingY = controllerHeadingY;
newStream.axisDeadband = axisDeadband;
newStream.translationAxisScale = translationAxisScale;
newStream.omegaAxisScale = omegaAxisScale;
newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled;
newStream.aimEnabled = aimEnabled;
newStream.currentMode = currentMode;
newStream.translationOnlyEnabled = translationOnlyEnabled;
newStream.lockedHeading = lockedHeading;
newStream.swerveController = swerveController;
return newStream;
}
/**
* Create a {@link SwerveInputStream} for an easy way to generate {@link ChassisSpeeds} from a driver controller.
*
@@ -193,6 +181,130 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
return new SwerveInputStream(drive, x, y);
}
/**
* Copy the {@link SwerveInputStream} object.
*
* @return Clone of current {@link SwerveInputStream}
*/
public SwerveInputStream copy()
{
SwerveInputStream newStream = new SwerveInputStream(swerveDrive, controllerTranslationX, controllerTranslationY);
newStream.controllerOmega = controllerOmega;
newStream.controllerHeadingX = controllerHeadingX;
newStream.controllerHeadingY = controllerHeadingY;
newStream.axisDeadband = axisDeadband;
newStream.translationAxisScale = translationAxisScale;
newStream.omegaAxisScale = omegaAxisScale;
newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled;
newStream.aimEnabled = aimEnabled;
newStream.currentMode = currentMode;
newStream.translationOnlyEnabled = translationOnlyEnabled;
newStream.lockedHeading = lockedHeading;
newStream.swerveController = swerveController;
newStream.omegaCube = omegaCube;
newStream.translationCube = translationCube;
newStream.robotRelative = robotRelative;
newStream.allianceRelative = allianceRelative;
return newStream;
}
/**
* Set the stream to output robot relative {@link ChassisSpeeds}
*
* @param enabled Robot-Relative {@link ChassisSpeeds} output.
* @return self
*/
public SwerveInputStream robotRelative(BooleanSupplier enabled)
{
robotRelative = Optional.of(enabled);
return this;
}
/**
* Set the stream to output robot relative {@link ChassisSpeeds}
*
* @param enabled Robot-Relative {@link ChassisSpeeds} output.
* @return self
*/
public SwerveInputStream robotRelative(boolean enabled)
{
robotRelative = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this;
}
/**
* Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance.
*
* @param enabled Alliance aware {@link ChassisSpeeds} output.
* @return self
*/
public SwerveInputStream allianceRelativeControl(BooleanSupplier enabled)
{
allianceRelative = Optional.of(enabled);
return this;
}
/**
* Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance.
*
* @param enabled Alliance aware {@link ChassisSpeeds} output.
* @return self
*/
public SwerveInputStream allianceRelativeControl(boolean enabled)
{
allianceRelative = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this;
}
/**
* Cube the angular velocity controller axis for a non-linear controls scheme.
*
* @param enabled Enabled state for the stream.
* @return self.
*/
public SwerveInputStream cubeRotationControllerAxis(BooleanSupplier enabled)
{
omegaCube = Optional.of(enabled);
return this;
}
/**
* Cube the angular velocity controller axis for a non-linear controls scheme.
*
* @param enabled Enabled state for the stream.
* @return self.
*/
public SwerveInputStream cubeRotationControllerAxis(boolean enabled)
{
omegaCube = Optional.of(() -> enabled);
return this;
}
/**
* Cube the translation axis magnitude for a non-linear control scheme.
*
* @param enabled Enabled state for the stream
* @return self
*/
public SwerveInputStream cubeTranslationControllerAxis(BooleanSupplier enabled)
{
translationOnlyEnabled = Optional.of(enabled);
return this;
}
/**
* Cube the translation axis magnitude for a non-linear control scheme
*
* @param enabled Enabled state for the stream
* @return self
*/
public SwerveInputStream cubeTranslationControllerAxis(boolean enabled)
{
translationCube = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this;
}
/**
* Add a rotation axis for Angular Velocity control
*
@@ -255,7 +367,6 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
return this;
}
/**
* Output {@link ChassisSpeeds} based on heading while the supplier is True.
*
@@ -510,9 +621,98 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
}
/**
* Gets a field-oriented {@link ChassisSpeeds}
* Apply the cube transformation on the given {@link Translation2d}
*
* @return field-oriented {@link ChassisSpeeds}
* @param translation {@link Translation2d} representing controller input
* @return Cubed {@link Translation2d} if the {@link SwerveInputStream#translationCube} is present.
*/
private Translation2d applyTranslationCube(Translation2d translation)
{
if (translationCube.isPresent() && translationCube.get().getAsBoolean())
{
return SwerveMath.cubeTranslation(translation);
}
return translation;
}
/**
* Apply the cube transformation on the given rotation controller axis
*
* @param rotationAxis Rotation controller axis to cube.
* @return Cubed axis value if the {@link SwerveInputStream#omegaCube} is present.
*/
private double applyOmegaCube(double rotationAxis)
{
if (omegaCube.isPresent() && omegaCube.get().getAsBoolean())
{
return Math.pow(rotationAxis, 3);
}
return rotationAxis;
}
/**
* Change {@link ChassisSpeeds} to robot relative.
*
* @param fieldRelativeSpeeds Field relative speeds to translate into robot-relative speeds.
* @return Robot relative {@link ChassisSpeeds}.
*/
private ChassisSpeeds applyRobotRelativeTranslation(ChassisSpeeds fieldRelativeSpeeds)
{
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
{
return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading());
}
return fieldRelativeSpeeds;
}
/**
* Apply alliance aware translation which flips the {@link Translation2d} if the robot is on the Blue alliance.
*
* @param fieldRelativeTranslation Field-relative {@link Translation2d} to flip.
* @return Alliance-oriented {@link Translation2d}
*/
private Translation2d applyAllianceAwareTranslation(Translation2d fieldRelativeTranslation)
{
if (allianceRelative.isPresent() && allianceRelative.get().getAsBoolean())
{
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
{
throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
}
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
{
return fieldRelativeTranslation.rotateBy(Rotation2d.k180deg);
}
}
return fieldRelativeTranslation;
}
/**
* Apply alliance aware translation which flips the {@link Rotation2d} if the robot is on the Blue alliance.
*
* @param fieldRelativeRotation Field-relative {@link Rotation2d} to flip.
* @return Alliance-oriented {@link Rotation2d}
*/
private Rotation2d applyAllianceAwareRotation(Rotation2d fieldRelativeRotation)
{
if (allianceRelative.isPresent() && allianceRelative.get().getAsBoolean())
{
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
{
throw new RuntimeException("Cannot use robot oriented control with Alliance aware movement!");
}
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red)
{
return fieldRelativeRotation.rotateBy(Rotation2d.k180deg);
}
}
return fieldRelativeRotation;
}
/**
* Gets a {@link ChassisSpeeds}
*
* @return {@link ChassisSpeeds}
*/
@Override
public ChassisSpeeds get()
@@ -520,10 +720,13 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
double maximumChassisVelocity = swerveDrive.getMaximumChassisVelocity();
Translation2d scaledTranslation = applyTranslationScalar(applyDeadband(controllerTranslationX.getAsDouble()),
applyDeadband(controllerTranslationY.getAsDouble()));
scaledTranslation = applyTranslationCube(scaledTranslation);
scaledTranslation = applyAllianceAwareTranslation(scaledTranslation);
double vxMetersPerSecond = scaledTranslation.getX() * maximumChassisVelocity;
double vyMetersPerSecond = scaledTranslation.getY() * maximumChassisVelocity;
double omegaRadiansPerSecond = 0;
double vxMetersPerSecond = scaledTranslation.getX() * maximumChassisVelocity;
double vyMetersPerSecond = scaledTranslation.getY() * maximumChassisVelocity;
double omegaRadiansPerSecond = 0;
ChassisSpeeds speeds = new ChassisSpeeds();
SwerveInputMode newMode = findMode();
// Handle transitions here.
@@ -542,21 +745,27 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
lockedHeading.get().getRadians());
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
case ANGULAR_VELOCITY ->
{
omegaRadiansPerSecond = applyRotationalScalar(applyDeadband(controllerOmega.get().getAsDouble())) *
omegaRadiansPerSecond = applyOmegaCube(applyRotationalScalar(applyDeadband(controllerOmega.get()
.getAsDouble()))) *
swerveDrive.getMaximumChassisAngularVelocity();
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
case HEADING ->
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
swerveController.getJoystickAngle(controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble()));
applyAllianceAwareRotation(Rotation2d.fromRadians(
swerveController.getJoystickAngle(
controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble()))).getRadians());
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
case AIM ->
@@ -565,12 +774,36 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
Translation2d relativeTrl = aimTarget.get().relativeTo(swerveDrive.getPose()).getTranslation();
Rotation2d target = new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(currentHeading);
omegaRadiansPerSecond = swerveController.headingCalculate(currentHeading.getRadians(), target.getRadians());
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
}
currentMode = newMode;
return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
return applyRobotRelativeTranslation(speeds);
}
/**
* Drive modes to keep track of.
*/
enum SwerveInputMode
{
/**
* Translation only mode, does not allow for rotation and maintains current heading.
*/
TRANSLATION_ONLY,
/**
* Output based off angular velocity
*/
ANGULAR_VELOCITY,
/**
* Output based off of heading.
*/
HEADING,
/**
* Output based off of targeting.
*/
AIM
}
}

28
swervelib/SwerveModule.java
View File

@@ -237,21 +237,21 @@ public class SwerveModule
moduleNumber,
AlertType.kWarning);
rawAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
rawAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Absolute Encoder").publish();
adjAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
adjAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder").publish();
absoluteEncoderIssuePublisher = NetworkTableInstance.getDefault().getBooleanTopic(
absoluteEncoderIssuePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getBooleanTopic(
"swerve/modules/" + configuration.name + "/Absolute Encoder Read Issue").publish();
rawAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
rawAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Angle Encoder").publish();
rawDriveEncoderPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
rawDriveEncoderPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Drive Encoder").publish();
rawDriveVelocityPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
rawDriveVelocityPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Drive Velocity").publish();
speedSetpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
speedSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Speed Setpoint").publish();
angleSetpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
angleSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
"swerve/modules/" + configuration.name + "/Angle Setpoint").publish();
}
@@ -423,7 +423,10 @@ public class SwerveModule
LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
desiredState.speedMetersPerSecond = nextVelocity.magnitude();
setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(nextVelocity).magnitude());
setDesiredState(desiredState,
isOpenLoop,
driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond),
nextVelocity.in(MetersPerSecond)));
}
/**
@@ -441,7 +444,7 @@ public class SwerveModule
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
driveMotor.set(percentOutput);
driveMotor.setVoltage(percentOutput * 12);
} else
{
driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage);
@@ -823,8 +826,9 @@ public class SwerveModule
* Configure the {@link SwerveModule#simModule} with the MapleSim
* {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
*
* @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to
* configure with.
* @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to
* configure with.
* @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} that represent the swerve drive.
*/
public void configureModuleSimulation(
org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation,

11
swervelib/encoders/CANCoderSwerve.java
View File

@@ -13,6 +13,7 @@ import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.Alert;
@@ -27,11 +28,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds);
/**
* CANCoder with WPILib sendable and support.
*/
public CANcoder encoder;
public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds);
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
@@ -60,6 +57,10 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
* Angular velocity of the {@link CANcoder}.
*/
private final StatusSignal<AngularVelocity> velocity;
/**
* CANCoder with WPILib sendable and support.
*/
public CANcoder encoder;
/**
* {@link CANcoder} Configurator objet for this class.
*/

12
swervelib/encoders/CanAndMagSwerve.java
View File

@@ -1,6 +1,7 @@
package swervelib.encoders;
import com.reduxrobotics.sensors.canandmag.Canandmag;
import com.reduxrobotics.sensors.canandmag.CanandmagSettings;
/**
* HELIUM {@link Canandmag} from ReduxRobotics absolute encoder, attached through the CAN bus.
@@ -12,6 +13,10 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
* The {@link Canandmag} representing the CANandMag on the CAN bus.
*/
public Canandmag encoder;
/**
* The {@link Canandmag} settings object to use.
*/
public CanandmagSettings settings;
/**
* Create the {@link Canandmag}
@@ -21,6 +26,7 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
public CanAndMagSwerve(int canid)
{
encoder = new Canandmag(canid);
settings = encoder.getSettings();
}
/**
@@ -51,7 +57,8 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
encoder.setSettings(new Canandmag.Settings().setInvertDirection(inverted));
settings.setInvertDirection(inverted);
encoder.setSettings(settings);
}
/**
@@ -85,7 +92,8 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
return encoder.setSettings(new Canandmag.Settings().setZeroOffset(offset));
settings.setZeroOffset(offset);
return encoder.setSettings(settings);
}
/**

94
swervelib/encoders/ThriftyNovaEncoderSwerve.java Normal file
View File

@@ -0,0 +1,94 @@
package swervelib.encoders;
import swervelib.motors.SwerveMotor;
import swervelib.motors.ThriftyNovaSwerve;
/**
* Thrifty Nova absolute encoder, attached through the data port.
*/
public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder {
/** The absolute encoder is directly interfaced through the Thrifty Nova motor. */
protected ThriftyNovaSwerve motor;
/**
* Inversion state of the attached encoder.
*/
protected boolean inverted = false;
/**
* Offset of the absolute encoder.
*/
protected double offset = 0.0;
/**
* Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve} motor.
*
* @param motor {@link SwerveMotor} through which to interface with the attached encoder .
*/
public ThriftyNovaEncoderSwerve(SwerveMotor motor) {
this.motor = (ThriftyNovaSwerve)motor;
motor.setAbsoluteEncoder(null);
}
/**
* Set factory default.
*/
@Override
public void factoryDefault() {
}
/**
* Clear sticky faults.
*/
@Override
public void clearStickyFaults() {
}
/**
* Configure the absolute encoder.
*
* @param inverted Whether the encoder is inverted.
*/
@Override
public void configure(boolean inverted) {
this.inverted = inverted;
}
/**
* Get the absolute position of the encoder.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition() {
return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
}
/**
* Get the instantiated absolute encoder Object.
*/
@Override
public Object getAbsoluteEncoder() {
return null;
}
/**
* Set the absolute encoder offset.
*
* @param offset offset in degrees from [0, 360).
* @return true if successful.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset) {
this.offset = offset;
return true;
}
/**
* Get the absolute encoder velocity.
* WARNING: Angular velocity is generally not measurable at high speeds.
* @return Velocity in degrees per second.
*/
@Override
public double getVelocity() {
return motor.getVelocity();
}
}

1
swervelib/imu/ADIS16448Swerve.java
View File

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.DegreesPerSecond;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

1
swervelib/imu/ADIS16470Swerve.java
View File

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.DegreesPerSecond;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;

1
swervelib/imu/ADXRS450Swerve.java
View File

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.DegreesPerSecond;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

1
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.DegreesPerSecond;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

2
swervelib/imu/CanandgyroSwerve.java
View File

@@ -5,8 +5,8 @@ import static edu.wpi.first.units.Units.RotationsPerSecond;
import com.reduxrobotics.sensors.canandgyro.Canandgyro;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import java.util.Optional;
/**

1
swervelib/imu/NavXSwerve.java
View File

@@ -6,7 +6,6 @@ import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;

1
swervelib/imu/Pigeon2Swerve.java
View File

@@ -8,7 +8,6 @@ import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.LinearAcceleration;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

1
swervelib/imu/PigeonSwerve.java
View File

@@ -6,7 +6,6 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU;
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.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;

6
swervelib/imu/SwerveIMU.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.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import java.util.Optional;
/**
@@ -59,9 +59,9 @@ public abstract class SwerveIMU
public abstract Optional<Translation3d> getAccel();
/**
* Fetch the rotation rate from the IMU as {@link AngularVelocity}
* Fetch the rotation rate from the IMU as {@link MutAngularVelocity}
*
* @return {@link AngularVelocity} of the rotation rate.
* @return {@link MutAngularVelocity} of the rotation rate.
*/
public abstract MutAngularVelocity getYawAngularVelocity();

4
swervelib/math/SwerveMath.java
View File

@@ -399,6 +399,10 @@ public class SwerveMath
*/
public static Translation2d cubeTranslation(Translation2d translation)
{
if (Math.hypot(translation.getX(), translation.getY()) <= 1.0E-6)
{
return translation;
}
return new Translation2d(Math.pow(translation.getNorm(), 3), translation.getAngle());
}

60
swervelib/motors/SparkFlexSwerve.java
View File

@@ -1,5 +1,6 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
@@ -8,6 +9,7 @@ import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
@@ -16,7 +18,6 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
@@ -41,7 +42,7 @@ public class SparkFlexSwerve extends SwerveMotor
*/
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
* Absolute encoder attached to the SparkFlex (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
/**
@@ -72,14 +73,6 @@ public class SparkFlexSwerve extends SwerveMotor
* Configuration object for {@link SparkFlex} motor.
*/
private SparkFlexConfig cfg = new SparkFlexConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
@@ -99,10 +92,9 @@ public class SparkFlexSwerve extends SwerveMotor
encoder = motor.getEncoder();
pid = motor.getClosedLoopController();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
cfgUpdated = true;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
// configureSparkFlex(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
@@ -118,7 +110,7 @@ public class SparkFlexSwerve extends SwerveMotor
/**
* Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor.
*
* @param id CAN ID of the SparkMax.
* @param id CAN ID of the SparkFlex.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} which the {@link SparkFlex} is attached to.
*/
@@ -140,7 +132,7 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
Timer.delay(Milliseconds.of(5).in(Seconds));
}
failureConfiguring.set(true);
}
@@ -148,7 +140,7 @@ public class SparkFlexSwerve extends SwerveMotor
/**
* Get the current configuration of the {@link SparkFlex}
*
* @return {@link SparkMaxConfig}
* @return {@link SparkFlexConfig}
*/
public SparkFlexConfig getConfig()
{
@@ -162,9 +154,12 @@ public class SparkFlexSwerve extends SwerveMotor
*/
public void updateConfig(SparkFlexConfig cfgGiven)
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
}
cfg.apply(cfgGiven);
configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
@@ -176,7 +171,6 @@ public class SparkFlexSwerve extends SwerveMotor
public void setVoltageCompensation(double nominalVoltage)
{
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -190,7 +184,6 @@ public class SparkFlexSwerve extends SwerveMotor
{
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -203,7 +196,6 @@ public class SparkFlexSwerve extends SwerveMotor
{
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -274,14 +266,12 @@ public class SparkFlexSwerve extends SwerveMotor
{
absoluteEncoder = null;
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
} else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
absoluteEncoderOffsetWarning.set(true);
absoluteEncoder = encoder;
@@ -361,7 +351,6 @@ public class SparkFlexSwerve extends SwerveMotor
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
@@ -376,7 +365,6 @@ public class SparkFlexSwerve extends SwerveMotor
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -391,7 +379,6 @@ public class SparkFlexSwerve extends SwerveMotor
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
}
@@ -404,7 +391,6 @@ public class SparkFlexSwerve extends SwerveMotor
public void setMotorBrake(boolean isBrakeMode)
{
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -416,7 +402,6 @@ public class SparkFlexSwerve extends SwerveMotor
public void setInverted(boolean inverted)
{
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -425,10 +410,13 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkFlex(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
/**
@@ -451,20 +439,6 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)
{
@@ -472,7 +446,7 @@ public class SparkFlexSwerve extends SwerveMotor
pid.setReference(
setpoint,
ControlType.kVelocity,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
} else
{
@@ -480,7 +454,7 @@ public class SparkFlexSwerve extends SwerveMotor
pid.setReference(
setpoint,
ControlType.kPosition,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
if (SwerveDriveTelemetry.isSimulation)
{

51
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -1,5 +1,6 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
@@ -8,6 +9,7 @@ import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
@@ -15,7 +17,6 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
@@ -78,14 +79,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
* Initialize the swerve motor.
@@ -152,7 +145,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
// Configure feedback of the PID controller as the integrated encoder.
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
}
cfgUpdated = true;
}
encoder.ifPresentOrElse((RelativeEncoder enc) -> {
velocity = enc::getVelocity;
@@ -195,7 +187,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
Timer.delay(Milliseconds.of(5).in(Seconds));
}
failureConfiguringAlert.set(true);
}
@@ -217,9 +209,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
*/
public void updateConfig(SparkMaxConfig cfgGiven)
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
}
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
@@ -231,7 +226,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void setVoltageCompensation(double nominalVoltage)
{
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -244,7 +238,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void setCurrentLimit(int currentLimit)
{
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
/**
@@ -257,7 +250,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
/**
@@ -328,7 +320,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
absoluteEncoder = null;
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
this.encoder.ifPresentOrElse((RelativeEncoder enc) -> {
velocity = enc::getVelocity;
@@ -341,7 +332,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
@@ -440,7 +430,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
/**
@@ -454,7 +443,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
/**
@@ -469,7 +457,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
}
@@ -482,7 +469,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void setMotorBrake(boolean isBrakeMode)
{
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
/**
@@ -494,7 +480,6 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void setInverted(boolean inverted)
{
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -503,10 +488,13 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
/**
@@ -531,26 +519,13 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kVelocity,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
} else
{
@@ -558,7 +533,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
pid.setReference(
setpoint,
ControlType.kPosition,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
if (SwerveDriveTelemetry.isSimulation)
{

51
swervelib/motors/SparkMaxSwerve.java
View File

@@ -1,10 +1,12 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
@@ -15,7 +17,6 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier;
@@ -63,14 +64,6 @@ public class SparkMaxSwerve extends SwerveMotor
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
@@ -92,7 +85,6 @@ public class SparkMaxSwerve extends SwerveMotor
pid = motor.getClosedLoopController();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder.
cfgUpdated = true;
velocity = encoder::getVelocity;
position = encoder::getPosition;
@@ -126,7 +118,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
Timer.delay(Milliseconds.of(5).in(Seconds));
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
@@ -148,9 +140,12 @@ public class SparkMaxSwerve extends SwerveMotor
*/
public void updateConfig(SparkMaxConfig cfgGiven)
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Configuration changes cannot be applied while the robot is enabled.");
}
cfg.apply(cfgGiven);
configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters));
cfgUpdated = false;
}
/**
@@ -162,7 +157,6 @@ public class SparkMaxSwerve extends SwerveMotor
public void setVoltageCompensation(double nominalVoltage)
{
cfg.voltageCompensation(nominalVoltage);
cfgUpdated = true;
}
/**
@@ -175,7 +169,6 @@ public class SparkMaxSwerve extends SwerveMotor
public void setCurrentLimit(int currentLimit)
{
cfg.smartCurrentLimit(currentLimit);
cfgUpdated = true;
}
@@ -189,7 +182,6 @@ public class SparkMaxSwerve extends SwerveMotor
{
cfg.closedLoopRampRate(rampRate)
.openLoopRampRate(rampRate);
cfgUpdated = true;
}
@@ -261,7 +253,6 @@ public class SparkMaxSwerve extends SwerveMotor
{
absoluteEncoder = null;
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfgUpdated = true;
velocity = this.encoder::getVelocity;
position = this.encoder::getPosition;
@@ -270,7 +261,6 @@ public class SparkMaxSwerve extends SwerveMotor
{
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
cfgUpdated = true;
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
@@ -365,7 +355,6 @@ public class SparkMaxSwerve extends SwerveMotor
.velocityConversionFactor(positionConversionFactor / 60);
}
}
cfgUpdated = true;
}
@@ -380,7 +369,6 @@ public class SparkMaxSwerve extends SwerveMotor
cfg.closedLoop.pidf(config.p, config.i, config.d, config.f)
.iZone(config.iz)
.outputRange(config.output.min, config.output.max);
cfgUpdated = true;
}
@@ -396,7 +384,6 @@ public class SparkMaxSwerve extends SwerveMotor
cfg.closedLoop
.positionWrappingEnabled(true)
.positionWrappingInputRange(minInput, maxInput);
cfgUpdated = true;
}
@@ -409,7 +396,6 @@ public class SparkMaxSwerve extends SwerveMotor
public void setMotorBrake(boolean isBrakeMode)
{
cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
cfgUpdated = true;
}
@@ -422,7 +408,6 @@ public class SparkMaxSwerve extends SwerveMotor
public void setInverted(boolean inverted)
{
cfg.inverted(inverted);
cfgUpdated = true;
}
/**
@@ -431,10 +416,13 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void burnFlash()
{
if (!DriverStation.isDisabled())
{
throw new RuntimeException("Config updates cannot be applied while the robot is Enabled!");
}
configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
/**
@@ -459,26 +447,13 @@ public class SparkMaxSwerve extends SwerveMotor
{
int pidSlot = 0;
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)
{
configureSparkMax(() ->
pid.setReference(
setpoint,
ControlType.kVelocity,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
} else
{
@@ -486,7 +461,7 @@ public class SparkMaxSwerve extends SwerveMotor
pid.setReference(
setpoint,
ControlType.kPosition,
pidSlot,
ClosedLoopSlot.kSlot0,
feedforward));
if (SwerveDriveTelemetry.isSimulation)
{

3
swervelib/motors/TalonFXSwerve.java
View File

@@ -189,9 +189,6 @@ public class TalonFXSwerve extends SwerveMotor
.withSensorToMechanismRatio(positionConversionFactor);
cfg.apply(configuration);
// Taken from democat's library.
// https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16
// configureCANStatusFrames(250);
}
/**

14
swervelib/motors/TalonSRXSwerve.java
View File

@@ -7,11 +7,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
import swervelib.parser.json.modules.ConversionFactorsJson;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
@@ -25,7 +25,7 @@ public class TalonSRXSwerve extends SwerveMotor
*/
private final boolean factoryDefaultOccurred = false;
/**
* Current TalonFX configuration.
* Current TalonSRX configuration.
*/
private final TalonSRXConfiguration configuration = new TalonSRXConfiguration();
/**
@@ -41,7 +41,11 @@ public class TalonSRXSwerve extends SwerveMotor
*/
private double positionConversionFactor = 1;
/**
* If the TalonFX configuration has changed.
* Module Conversion factors to use.
*/
private ConversionFactorsJson moduleConversionFactors;
/**
* If the TalonSRX configuration has changed.
*/
private boolean configChanged = true;
/**
@@ -73,7 +77,7 @@ public class TalonSRXSwerve extends SwerveMotor
*
* @param id ID of the TalonSRX on the canbus.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFX} is attached to.
* @param motorType {@link DCMotor} which the {@link WPI_TalonSRX} is attached to.
*/
public TalonSRXSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
@@ -468,4 +472,4 @@ public class TalonSRXSwerve extends SwerveMotor
{
return absoluteEncoder;
}
}
}

448
swervelib/motors/ThriftyNovaSwerve.java Normal file
View File

@@ -0,0 +1,448 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package swervelib.motors;
import com.thethriftybot.Conversion;
import com.thethriftybot.Conversion.PositionUnit;
import com.thethriftybot.Conversion.VelocityUnit;
import com.thethriftybot.ThriftyNova;
import com.thethriftybot.ThriftyNova.CurrentType;
import com.thethriftybot.ThriftyNova.EncoderType;
import com.thethriftybot.ThriftyNova.PIDSlot;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import java.util.List;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
/**
* An implementation of {@link ThriftyNova} as a {@link SwerveMotor}.
*/
public class ThriftyNovaSwerve extends SwerveMotor
{
/**
* ThriftyNova Instance.
*/
private ThriftyNova motor;
/**
* The Encoder type being used
*/
private EncoderType encoderType;
/**
* Closed-loop PID controller.
*/
public PIDController pid;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
/**
* Position conversion object for the motor encoder
*/
private Conversion positionConversion;
/**
* Velocity conversion object for the motor encoder
*/
private Conversion velocityConversion;
/**
* The position conversion factor for the encoder
*/
private double positionConversionFactor = 1.0;
/**
* The position conversion factor for the encoder
*/
private double velocityConversionFactor = 1.0 / 60.0;
/**
* {@link DCMotor} for simulation and calculations.
*/
private final DCMotor simMotor;
/**
* Initialize the swerve motor.
*
* @param motor The SwerveMotor as a ThriftyNova object.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motorType {@link DCMotor} controlled by the {@link ThriftyNova}
*/
public ThriftyNovaSwerve(ThriftyNova motor, boolean isDriveMotor, DCMotor motorType)
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
this.simMotor = motorType;
factoryDefaults();
clearStickyFaults();
motor.usePIDSlot(PIDSlot.SLOT0);
pid = new PIDController(0, 0, 0);
motor.pid0.setPID(pid);
if (isDriveMotor)
{
positionConversion = new Conversion(PositionUnit.ROTATIONS, EncoderType.INTERNAL);
velocityConversion = new Conversion(VelocityUnit.ROTATIONS_PER_SEC, EncoderType.INTERNAL);
} else
{
positionConversion = new Conversion(PositionUnit.DEGREES, EncoderType.INTERNAL);
velocityConversion = new Conversion(VelocityUnit.DEGREES_PER_SEC, EncoderType.INTERNAL);
}
}
/**
* Initialize the {@link SwerveMotor} as a {@link ThriftyNova} connected to a Brushless Motor.
*
* @param id CAN ID of the ThriftyNova.
* @param isDriveMotor Is the motor being initialized a drive motor?
* @param motor {@link DCMotor} controlled by the {@link ThriftyNova}
*/
public ThriftyNovaSwerve(int id, boolean isDriveMotor, DCMotor motor)
{
this(new ThriftyNova(id), isDriveMotor, motor);
}
/**
* Set factory defaults on the motor controller.
*/
@Override
public void factoryDefaults()
{
// Factory defaults from https://docs.thethriftybot.com/thrifty-nova/gqCPUYXcVoOZ4KW3DqIr/software-resources/configure-controller-settings/factory-default
if (!factoryDefaultOccurred)
{
motor.setInverted(false);
motor.setBrakeMode(false);
setCurrentLimit(40);
motor.setEncoderPosition(0);
motor.setMaxOutput(1.0);
motor.setRampDown(100);
motor.setRampUp(100);
configureCANStatusFrames(0.25, 0.1, 0.25, 0.5, 0.50);
motor.setSoftLimits(0, 0);
configurePIDF(new PIDFConfig());
motor.pid1.setP(0)
.setI(0)
.setD(0)
.setFF(0.0);
DriverStation.reportWarning("Factory defaults not implemented for ThriftyNovaSwerve", true);
factoryDefaultOccurred = true;
}
}
/**
* Clear the sticky faults on the motor controller.
*/
@Override
public void clearStickyFaults()
{
motor.clearErrors();
}
/**
* Set the absolute encoder to be a compatible absolute encoder.
*
* @param encoder The encoder to use.
* @return The {@link SwerveMotor} for easy instantiation.
*/
@Override
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
{
if (isDriveMotor)
{
positionConversion = new Conversion(PositionUnit.ROTATIONS, EncoderType.ABS);
velocityConversion = new Conversion(VelocityUnit.ROTATIONS_PER_SEC, EncoderType.ABS);
} else
{
positionConversion = new Conversion(PositionUnit.DEGREES, EncoderType.ABS);
velocityConversion = new Conversion(VelocityUnit.DEGREES_PER_SEC, EncoderType.ABS);
}
motor.useEncoderType(EncoderType.ABS);
return this;
}
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
motor.useEncoderType(EncoderType.INTERNAL);
if (isDriveMotor)
{
positionConversion = new Conversion(PositionUnit.ROTATIONS, EncoderType.INTERNAL);
velocityConversion = new Conversion(VelocityUnit.ROTATIONS_PER_SEC, EncoderType.INTERNAL);
} else
{
positionConversion = new Conversion(PositionUnit.DEGREES, EncoderType.INTERNAL);
velocityConversion = new Conversion(VelocityUnit.DEGREES_PER_SEC, EncoderType.INTERNAL);
}
this.positionConversionFactor = positionConversionFactor;
this.velocityConversionFactor = positionConversionFactor / 60.0;
configureCANStatusFrames(0.25, 0.01, 0.01, 0.02, 0.20);
}
/**
* Set the CAN status frames.
*
* @param fault Fault transmission rate
* @param sensor Sensor transmission rate
* @param quadSensor External quad encoder transmission rate
* @param control Control frame transmission rate
* @param current Current feedback transmission rate
*/
public void configureCANStatusFrames(
double fault, double sensor, double quadSensor, double control, double current)
{
motor.canFreq.setFault(fault);
motor.canFreq.setSensor(sensor);
motor.canFreq.setQuadSensor(quadSensor);
motor.canFreq.setControl(control);
motor.canFreq.setCurrent(current);
checkErrors("Configuring CAN status frames failed: ");
}
/**
* Configure the PIDF values for the closed loop controller. 0 is disabled or off.
*
* @param config Configuration class holding the PIDF values.
*/
@Override
public void configurePIDF(PIDFConfig config)
{
motor.pid0.setP(config.p).setI(config.i).setD(config.d);
checkErrors("Configuring PIDF failed: ");
}
/**
* Configure the PID wrapping for the position closed loop controller.
*
* @param minInput Minimum PID input.
* @param maxInput Maximum PID input.
*/
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
// Do nothing
}
/**
* Set the idle mode.
*
* @param isBrakeMode Set the brake mode.
*/
@Override
public void setMotorBrake(boolean isBrakeMode)
{
motor.setBrakeMode(isBrakeMode);
checkErrors("Setting motor brake mode failed: ");
}
/**
* Set the motor to be inverted.
*
* @param inverted State of inversion.
*/
@Override
public void setInverted(boolean inverted)
{
motor.setInverted(inverted);
checkErrors("Setting motor inversion failed: ");
}
/**
* Save the configurations from flash to EEPROM.
*/
@Override
public void burnFlash()
{
// Do nothing
}
/**
* Set the percentage output.
*
* @param percentOutput percent out for the motor controller.
*/
@Override
public void set(double percentOutput)
{
motor.setPercent(percentOutput);
}
/**
* Set the closed loop PID controller reference point.
*
* @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)
{
setReference(setpoint, feedforward, getPosition());
}
/**
* Set the closed loop PID controller reference point.
*
* @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.
*/
@Override
public void setReference(double setpoint, double feedforward, double position)
{
if (isDriveMotor)
{
motor.pid0.setFF(feedforward);
motor.setVelocity(setpoint);
} else
{
motor.setPosition(setpoint / 360.0);
}
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
throw new UnsupportedOperationException("Unimplemented method 'getVoltage'");
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setPercent(voltage / RobotController.getBatteryVoltage());
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getAppliedOutput()
{
return motor.getStatorCurrent();
}
/**
* Get the velocity of the integrated encoder.
*
* @return velocity in Meters Per Second, or Degrees per Second.
*/
@Override
public double getVelocity()
{
return velocityConversion.fromMotor(motor.getVelocity()) * velocityConversionFactor;
}
/**
* Get the position of the integrated encoder.
*
* @return Position in Meters or Degrees.
*/
@Override
public double getPosition()
{
return positionConversion.fromMotor(motor.getPosition()) * positionConversionFactor;
}
/**
* Set the integrated encoder position.
*
* @param position Integrated encoder position. Should be angle in degrees or meters.
*/
@Override
public void setPosition(double position)
{
motor.setEncoderPosition(positionConversion.toMotor(position));
}
@Override
public void setVoltageCompensation(double nominalVoltage)
{
}
/**
* 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)
{
motor.setMaxCurrent(CurrentType.STATOR, currentLimit);
checkErrors("Setting current limit failed: ");
}
/**
* Set the maximum rate the open/closed loop output can change by.
*
* @param rampRate Time in seconds to go from 0 to full throttle.
*/
@Override
public void setLoopRampRate(double rampRate)
{
motor.setRampUp(rampRate);
motor.setRampDown(rampRate);
checkErrors("Setting loop ramp rate failed: ");
}
/**
* Get the motor object from the module.
*
* @return Motor object.
*/
@Override
public Object getMotor()
{
return motor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
{
return EncoderType.ABS == encoderType;
}
/**
* Checks for errors in the motor and logs them if any are found.
*
* @param message the message to prepend to the log and print statement
*/
private void checkErrors(String message)
{
List<ThriftyNova.Error> errors = motor.getErrors();
if (errors.size() > 0)
{
for (ThriftyNova.Error error : errors)
{
DataLogManager.log(this.getClass().getSimpleName() + ": " + message + error.toString());
System.out.println(this.getClass().getSimpleName() + ": " + message + error.toString());
}
}
}
@Override
public DCMotor getSimMotor()
{
return simMotor;
}
}

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

@@ -6,6 +6,7 @@ import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
@@ -16,6 +17,7 @@ import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.encoders.TalonSRXEncoderSwerve;
import swervelib.encoders.ThriftyNovaEncoderSwerve;
import swervelib.imu.ADIS16448Swerve;
import swervelib.imu.ADIS16470Swerve;
import swervelib.imu.ADXRS450Swerve;
@@ -32,6 +34,7 @@ import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSwerve;
import swervelib.motors.TalonSRXSwerve;
import swervelib.motors.ThriftyNovaSwerve;
/**
* Device JSON parsed class. Used to access the JSON data.
@@ -97,6 +100,8 @@ public class DeviceJson
return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
case "talonsrx_analog":
return new TalonSRXEncoderSwerve(motor, FeedbackDevice.Analog);
case "thrifty_nova":
return new ThriftyNovaEncoderSwerve(motor);
default:
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
}
@@ -221,6 +226,10 @@ public class DeviceJson
// We are creating a motor for an angle motor which will use the absolute encoder attached to the data port.
return new SparkMaxBrushedMotorSwerve(id, isDriveMotor, Type.kNoSensor, 0, false, DCMotor.getCIM(1));
}
case "nova_neo":
return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "nova_neo550":
return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
default:
throw new RuntimeException(type + " is not a recognized motor type.");
}

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

@@ -4,7 +4,9 @@ import com.revrobotics.spark.SparkMax;
import edu.wpi.first.math.util.Units;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.encoders.ThriftyNovaEncoderSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.ThriftyNovaSwerve;
import swervelib.parser.PIDFConfig;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
@@ -103,7 +105,10 @@ public class ModuleJson
// Backwards compatibility, auto-optimization.
if (conversionFactors.angle.factor == 360 && absEncoder != null &&
absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)
(absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax))
{
angleMotor.setAbsoluteEncoder(absEncoder);
} else if ((absEncoder instanceof ThriftyNovaEncoderSwerve && angleMotor instanceof ThriftyNovaSwerve))
{
angleMotor.setAbsoluteEncoder(absEncoder);
}

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

@@ -1,6 +1,8 @@
package swervelib.parser.json;
import edu.wpi.first.units.Units;
import static edu.wpi.first.units.Units.Kilogram;
import static edu.wpi.first.units.Units.Pounds;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
@@ -80,7 +82,7 @@ public class PhysicalPropertiesJson
friction.drive,
friction.angle,
steerRotationalInertia,
Units.Pounds.of(robotMass).in(Units.Kilogram));
Pounds.of(robotMass).in(Kilogram));
}
}

1
swervelib/simulation/SwerveModuleSimulation.java
View File

@@ -20,6 +20,7 @@ public class SwerveModuleSimulation
* Configure the maple sim module
*
* @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation
* @param physicalCharacteristics Physical characteristics of the swerve drive from the JSON or built.
*/
public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule,
SwerveModulePhysicalCharacteristics physicalCharacteristics)

140
swervelib/telemetry/SwerveDriveTelemetry.java
View File

@@ -45,6 +45,8 @@ public class SwerveDriveTelemetry
*/
private static final DoublePublisher moduleCountPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/moduleCount")
.publish();
@@ -53,6 +55,8 @@ public class SwerveDriveTelemetry
*/
private static final DoubleArrayPublisher measuredStatesArrayPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleArrayTopic(
"swerve/measuredStates")
.publish();
@@ -61,6 +65,8 @@ public class SwerveDriveTelemetry
*/
private static final DoubleArrayPublisher desiredStatesArrayPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleArrayTopic(
"swerve/desiredStates")
.publish();
@@ -69,6 +75,8 @@ public class SwerveDriveTelemetry
*/
private static final DoubleArrayPublisher measuredChassisSpeedsArrayPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleArrayTopic(
"swerve/measuredChassisSpeeds")
.publish();
@@ -77,6 +85,8 @@ public class SwerveDriveTelemetry
*/
private static final DoubleArrayPublisher desiredChassisSpeedsArrayPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleArrayTopic(
"swerve/desiredChassisSpeeds")
.publish();
@@ -85,6 +95,8 @@ public class SwerveDriveTelemetry
*/
private static final DoublePublisher robotRotationPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/robotRotation")
.publish();
@@ -93,6 +105,8 @@ public class SwerveDriveTelemetry
*/
private static final DoublePublisher maxAngularVelocityPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/maxAngularVelocity")
.publish();
@@ -101,6 +115,8 @@ public class SwerveDriveTelemetry
*/
private static final StructArrayPublisher<SwerveModuleState> measuredStatesStruct
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStructArrayTopic(
"swerve/advantagescope/currentStates",
SwerveModuleState.struct)
@@ -110,6 +126,8 @@ public class SwerveDriveTelemetry
*/
private static final StructArrayPublisher<SwerveModuleState> desiredStatesStruct
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStructArrayTopic(
"swerve/advantagescope/desiredStates",
SwerveModuleState.struct)
@@ -119,6 +137,8 @@ public class SwerveDriveTelemetry
*/
private static final StructPublisher<ChassisSpeeds> measuredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStructTopic(
"swerve/advantagescope/measuredChassisSpeeds",
ChassisSpeeds.struct)
@@ -128,6 +148,8 @@ public class SwerveDriveTelemetry
*/
private static final StructPublisher<ChassisSpeeds> desiredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStructTopic(
"swerve/advantagescope/desiredChassisSpeeds",
ChassisSpeeds.struct)
@@ -137,6 +159,8 @@ public class SwerveDriveTelemetry
*/
private static final StructPublisher<Rotation2d> robotRotationStruct
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStructTopic(
"swerve/advantagescope/robotRotation",
Rotation2d.struct)
@@ -144,103 +168,125 @@ public class SwerveDriveTelemetry
/**
* Wheel locations array publisher for NT4.
*/
private static final DoubleArrayPublisher wheelLocationsArrayPublisher = NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/wheelLocation")
.publish();
private static final DoubleArrayPublisher wheelLocationsArrayPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleArrayTopic(
"swerve/wheelLocation")
.publish();
/**
* Max speed publisher for NT4.
*/
private static final DoublePublisher maxSpeedPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/maxSpeed")
.publish();
private static final DoublePublisher maxSpeedPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/maxSpeed")
.publish();
/**
* Rotation unit for NT4.
*/
private static final StringPublisher rotationUnitPublisher = NetworkTableInstance.getDefault()
.getStringTopic(
"swerve/rotationUnit")
.publish();
private static final StringPublisher rotationUnitPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStringTopic(
"swerve/rotationUnit")
.publish();
/**
* Chassis width publisher
*/
private static final DoublePublisher sizeLeftRightPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/sizeLeftRight")
.publish();
private static final DoublePublisher sizeLeftRightPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/sizeLeftRight")
.publish();
/**
* Chassis Length publisher.
*/
private static final DoublePublisher sizeFrontBackPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/sizeFrontBack")
.publish();
private static final DoublePublisher sizeFrontBackPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/sizeFrontBack")
.publish();
/**
* Chassis direction widget publisher.
*/
private static final StringPublisher forwardDirectionPublisher = NetworkTableInstance.getDefault()
.getStringTopic(
"swerve/forwardDirection")
.publish();
private static final StringPublisher forwardDirectionPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getStringTopic(
"swerve/forwardDirection")
.publish();
/**
* Odometry cycle time, updated whenever {@link SwerveDrive#updateOdometry()} is called.
*/
private static final DoublePublisher odomCycleTime
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/odomCycleMS")
.publish();
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/odomCycleMS")
.publish();
/**
* Control cycle time, updated whenever
* {@link swervelib.SwerveModule#setDesiredState(SwerveModuleState, boolean, double)} is called for the last module.
*/
private static final DoublePublisher ctrlCycleTime
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/controlCycleMS")
.publish();
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/controlCycleMS")
.publish();
/**
* Odometry timer to track cycle times.
*/
private static final Timer odomTimer = new Timer();
private static final Timer odomTimer = new Timer();
/**
* Control timer to track cycle times.
*/
private static final Timer ctrlTimer = new Timer();
private static final Timer ctrlTimer = new Timer();
/**
* Measured swerve module states object.
*/
public static SwerveModuleState[] measuredStatesObj
= new SwerveModuleState[4];
= new SwerveModuleState[4];
/**
* Desired swerve module states object
*/
public static SwerveModuleState[] desiredStatesObj
= new SwerveModuleState[4];
= new SwerveModuleState[4];
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static Rotation2d robotRotationObj = new Rotation2d();
public static Rotation2d robotRotationObj = new Rotation2d();
/**
* The current telemetry verbosity level.
*/
public static TelemetryVerbosity verbosity
= TelemetryVerbosity.MACHINE;
= TelemetryVerbosity.MACHINE;
/**
* State of simulation of the Robot, used to optimize retrieval.
*/
public static boolean isSimulation
= RobotBase.isSimulation();
= RobotBase.isSimulation();
/**
* The number of swerve modules
*/
@@ -260,7 +306,7 @@ public class SwerveDriveTelemetry
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static double robotRotation = 0;
public static double robotRotation = 0;
/**
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
*/
@@ -268,7 +314,7 @@ public class SwerveDriveTelemetry
/**
* The units of the module rotations and robot rotation
*/
public static String rotationUnit = "degrees";
public static String rotationUnit = "degrees";
/**
* The distance between the left and right modules.
*/
@@ -281,7 +327,7 @@ public class SwerveDriveTelemetry
* The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to
* align with odometry data or match videos. 'up', 'right', 'down' or 'left'
*/
public static String forwardDirection = "up";
public static String forwardDirection = "up";
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
@@ -291,15 +337,15 @@ public class SwerveDriveTelemetry
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static double[] measuredChassisSpeeds = new double[3];
public static double[] measuredChassisSpeeds = new double[3];
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static double[] desiredChassisSpeeds = new double[3];
public static double[] desiredChassisSpeeds = new double[3];
/**
* Update the telemetry settings that infrequently change.
*/
public static boolean updateSettings = true;
public static boolean updateSettings = true;
/**
* Start the ctrl timer to measure cycle time, independent of periodic loops.