diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index ad09420..c5c4ff5 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -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.
- *

* * @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; diff --git a/swervelib/SwerveInputStream.java b/swervelib/SwerveInputStream.java index 4a8748d..0222bf0 100644 --- a/swervelib/SwerveInputStream.java +++ b/swervelib/SwerveInputStream.java @@ -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.
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} *

- * Intended to easily create an interface that generates {@link ChassisSpeeds} from - * {@link edu.wpi.first.wpilibj.XboxController} + *
Inspired by SciBorgs FRC 1155.
Example: + * 

+ * {@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.
+ * }
+ *
*/ public class SwerveInputStream implements Supplier { @@ -37,7 +56,7 @@ public class SwerveInputStream implements Supplier /** * Rotation supplier as angular velocity. */ - private Optional controllerOmega = Optional.empty(); + private Optional controllerOmega = Optional.empty(); /** * Controller supplier as heading. */ @@ -49,19 +68,19 @@ public class SwerveInputStream implements Supplier /** * Axis deadband for the controller. */ - private Optional axisDeadband = Optional.empty(); + private Optional axisDeadband = Optional.empty(); /** * Translational axis scalar value, should be between (0, 1]. */ - private Optional translationAxisScale = Optional.empty(); + private Optional translationAxisScale = Optional.empty(); /** * Angular velocity axis scalar value, should be between (0, 1] */ - private Optional omegaAxisScale = Optional.empty(); + private Optional omegaAxisScale = Optional.empty(); /** * Target to aim at. */ - private Optional aimTarget = Optional.empty(); + private Optional aimTarget = Optional.empty(); /** * Output {@link ChassisSpeeds} based on heading while this is True. */ @@ -78,6 +97,22 @@ public class SwerveInputStream implements Supplier * Maintain current heading and drive without rotating, ideally. */ private Optional translationOnlyEnabled = Optional.empty(); + /** + * Cube the translation magnitude from the controller. + */ + private Optional translationCube = Optional.empty(); + /** + * Cube the angular velocity axis from the controller. + */ + private Optional omegaCube = Optional.empty(); + /** + * Robot relative oriented output expected. + */ + private Optional robotRelative = Optional.empty(); + /** + * Field oriented chassis output is relative to your current alliance. + */ + private Optional allianceRelative = Optional.empty(); /** * {@link SwerveController} for simple control over heading. */ @@ -88,53 +123,6 @@ public class SwerveInputStream implements Supplier 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 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 return this; } - /** * Output {@link ChassisSpeeds} based on heading while the supplier is True. * @@ -510,9 +621,98 @@ public class SwerveInputStream implements Supplier } /** - * 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 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 { 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 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 } } diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index ddd694d..81bb658 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -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, diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index 3789dc3..ba21689 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -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 velocity; + /** + * CANCoder with WPILib sendable and support. + */ + public CANcoder encoder; /** * {@link CANcoder} Configurator objet for this class. */ diff --git a/swervelib/encoders/CanAndMagSwerve.java b/swervelib/encoders/CanAndMagSwerve.java index 91000c3..4297158 100644 --- a/swervelib/encoders/CanAndMagSwerve.java +++ b/swervelib/encoders/CanAndMagSwerve.java @@ -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); } /** diff --git a/swervelib/encoders/ThriftyNovaEncoderSwerve.java b/swervelib/encoders/ThriftyNovaEncoderSwerve.java new file mode 100644 index 0000000..36292a8 --- /dev/null +++ b/swervelib/encoders/ThriftyNovaEncoderSwerve.java @@ -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(); + } +} diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java index 8447fc6..fc5f636 100644 --- a/swervelib/imu/ADIS16448Swerve.java +++ b/swervelib/imu/ADIS16448Swerve.java @@ -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; diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java index 620710d..79603e0 100644 --- a/swervelib/imu/ADIS16470Swerve.java +++ b/swervelib/imu/ADIS16470Swerve.java @@ -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; diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java index 033ad3e..d2be03f 100644 --- a/swervelib/imu/ADXRS450Swerve.java +++ b/swervelib/imu/ADXRS450Swerve.java @@ -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; diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java index a3afaf1..168c365 100644 --- a/swervelib/imu/AnalogGyroSwerve.java +++ b/swervelib/imu/AnalogGyroSwerve.java @@ -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; diff --git a/swervelib/imu/CanandgyroSwerve.java b/swervelib/imu/CanandgyroSwerve.java index 5d5f67a..357dc0c 100644 --- a/swervelib/imu/CanandgyroSwerve.java +++ b/swervelib/imu/CanandgyroSwerve.java @@ -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; /** diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index a99450e..64f97d0 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -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; diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index c471776..752a3b9 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -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; diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java index 12ccf78..81eb2da 100644 --- a/swervelib/imu/PigeonSwerve.java +++ b/swervelib/imu/PigeonSwerve.java @@ -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; diff --git a/swervelib/imu/SwerveIMU.java b/swervelib/imu/SwerveIMU.java index 2b23036..3d58d7d 100644 --- a/swervelib/imu/SwerveIMU.java +++ b/swervelib/imu/SwerveIMU.java @@ -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 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(); diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index eea10e4..7fa8f71 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -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()); } diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index ecbcf26..eab749f 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -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) { diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 26fb76c..db3ea10 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -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) { diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index 4b2955b..5cf8515 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -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) { diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index 73393ce..7d3b98a 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -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); } /** diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 5c8f690..00958e8 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -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; } -} +} \ No newline at end of file diff --git a/swervelib/motors/ThriftyNovaSwerve.java b/swervelib/motors/ThriftyNovaSwerve.java new file mode 100644 index 0000000..17b6a75 --- /dev/null +++ b/swervelib/motors/ThriftyNovaSwerve.java @@ -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 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; + } +} diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 1d6f339..7c10b6b 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -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."); } diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index f26b2e8..7cb8821 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -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); } diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java index eaab95d..758675d 100644 --- a/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/swervelib/parser/json/PhysicalPropertiesJson.java @@ -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)); } } diff --git a/swervelib/simulation/SwerveModuleSimulation.java b/swervelib/simulation/SwerveModuleSimulation.java index 76b15ec..96fe383 100644 --- a/swervelib/simulation/SwerveModuleSimulation.java +++ b/swervelib/simulation/SwerveModuleSimulation.java @@ -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) diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java index 5a7c382..8bd5964 100644 --- a/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/swervelib/telemetry/SwerveDriveTelemetry.java @@ -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 measuredStatesStruct = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getStructArrayTopic( "swerve/advantagescope/currentStates", SwerveModuleState.struct) @@ -110,6 +126,8 @@ public class SwerveDriveTelemetry */ private static final StructArrayPublisher desiredStatesStruct = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getStructArrayTopic( "swerve/advantagescope/desiredStates", SwerveModuleState.struct) @@ -119,6 +137,8 @@ public class SwerveDriveTelemetry */ private static final StructPublisher measuredChassisSpeedsStruct = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getStructTopic( "swerve/advantagescope/measuredChassisSpeeds", ChassisSpeeds.struct) @@ -128,6 +148,8 @@ public class SwerveDriveTelemetry */ private static final StructPublisher desiredChassisSpeedsStruct = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getStructTopic( "swerve/advantagescope/desiredChassisSpeeds", ChassisSpeeds.struct) @@ -137,6 +159,8 @@ public class SwerveDriveTelemetry */ private static final StructPublisher 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.