From 4016ee219003baabd6fe0ff481af86314ded9c42 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Sat, 22 Feb 2025 06:15:56 +0000 Subject: [PATCH] Upgrading to 2025.7.0 --- swervelib/SwerveDrive.java | 142 ++++-- swervelib/SwerveDriveTest.java | 99 +++- swervelib/SwerveInputStream.java | 273 ++++++++-- swervelib/SwerveModule.java | 249 ++++++++-- .../encoders/AnalogAbsoluteEncoderSwerve.java | 6 + swervelib/encoders/CANCoderSwerve.java | 17 +- swervelib/encoders/CanAndMagSwerve.java | 8 +- .../encoders/DIODutyCycleEncoderSwerve.java | 129 +++++ .../encoders/SparkFlexEncoderSwerve.java | 165 +++++++ .../encoders/SparkMaxAnalogEncoderSwerve.java | 60 ++- swervelib/encoders/SparkMaxEncoderSwerve.java | 78 ++- swervelib/encoders/SwerveAbsoluteEncoder.java | 8 +- swervelib/encoders/TalonSRXEncoderSwerve.java | 9 + .../encoders/ThriftyNovaEncoderSwerve.java | 178 ++++--- swervelib/imu/ADIS16448Swerve.java | 7 +- swervelib/imu/ADIS16470Swerve.java | 7 +- swervelib/imu/ADXRS450Swerve.java | 7 +- swervelib/imu/AnalogGyroSwerve.java | 7 +- swervelib/imu/CanandgyroSwerve.java | 9 +- swervelib/imu/NavXSwerve.java | 6 +- swervelib/imu/Pigeon2Swerve.java | 56 ++- swervelib/imu/PigeonSwerve.java | 8 +- swervelib/imu/PigeonViaTalonSRXSwerve.java | 158 ++++++ swervelib/imu/SwerveIMU.java | 6 +- swervelib/math/SwerveMath.java | 2 +- swervelib/motors/SparkFlexSwerve.java | 146 +++--- .../motors/SparkMaxBrushedMotorSwerve.java | 150 +++--- swervelib/motors/SparkMaxSwerve.java | 141 +++--- swervelib/motors/SwerveMotor.java | 12 +- swervelib/motors/TalonFXSSwerve.java | 467 ++++++++++++++++++ swervelib/motors/TalonFXSwerve.java | 19 +- swervelib/motors/TalonSRXSwerve.java | 20 +- swervelib/motors/ThriftyNovaSwerve.java | 37 +- swervelib/parser/PIDFConfig.java | 6 +- swervelib/parser/SwerveParser.java | 9 +- swervelib/parser/json/DeviceJson.java | 36 +- swervelib/parser/json/ModuleJson.java | 12 +- .../parser/json/PhysicalPropertiesJson.java | 5 +- .../parser/json/modules/LocationJson.java | 4 +- .../simulation/SwerveModuleSimulation.java | 34 +- swervelib/telemetry/SwerveDriveTelemetry.java | 2 +- 41 files changed, 2237 insertions(+), 557 deletions(-) create mode 100644 swervelib/encoders/DIODutyCycleEncoderSwerve.java create mode 100644 swervelib/encoders/SparkFlexEncoderSwerve.java create mode 100644 swervelib/imu/PigeonViaTalonSRXSwerve.java create mode 100644 swervelib/motors/TalonFXSSwerve.java diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index c5c4ff5..7182215 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Newtons; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -52,14 +51,11 @@ 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; @@ -75,7 +71,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** * Swerve Drive class representing and controlling the swerve drive. */ -public class SwerveDrive +public class SwerveDrive implements AutoCloseable { /** @@ -118,16 +114,20 @@ public class SwerveDrive */ private final DoublePublisher rawIMUPublisher = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getDoubleTopic( - "swerve/Raw IMU Yaw") + "swerve/imu/raw") .publish(); /** * NT4 Publisher for the IMU reading adjusted by offset and inversion. */ private final DoublePublisher adjustedIMUPublisher = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") .getDoubleTopic( - "swerve/Adjusted IMU Yaw") + "swerve/imu/adjusted") .publish(); /** * Field object. @@ -224,7 +224,9 @@ public class SwerveDrive SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS, Pose2d startingPose) { - this.maxChassisSpeedMPS = maxSpeedMPS; + this.attainableMaxTranslationalSpeedMetersPerSecond = this.maxChassisSpeedMPS = maxSpeedMPS; + this.attainableMaxRotationalVelocityRadiansPerSecond = Math.PI * + 2; // Defaulting to something reasonable for most robots swerveDriveConfiguration = config; swerveController = new SwerveController(controllerConfig); // Create Kinematics from swerve module locations. @@ -247,17 +249,17 @@ public class SwerveDrive .withCustomModuleTranslations(config.moduleLocationsMeters) .withGyro(config.getGyroSim()) .withSwerveModule(new SwerveModuleSimulationConfig( - config.getDriveMotorSim(), - config.getAngleMotorSim(), - config.physicalCharacteristics.conversionFactor.drive.gearRatio, - config.physicalCharacteristics.conversionFactor.angle.gearRatio, - Volts.of(config.physicalCharacteristics.driveFrictionVoltage), - Volts.of(config.physicalCharacteristics.angleFrictionVoltage), - Inches.of( - config.physicalCharacteristics.conversionFactor.drive.diameter / - 2), - KilogramSquareMeters.of(0.02), - config.physicalCharacteristics.wheelGripCoefficientOfFriction) + config.getDriveMotorSim(), + config.getAngleMotorSim(), + config.physicalCharacteristics.conversionFactor.drive.gearRatio, + config.physicalCharacteristics.conversionFactor.angle.gearRatio, + Volts.of(config.physicalCharacteristics.driveFrictionVoltage), + Volts.of(config.physicalCharacteristics.angleFrictionVoltage), + Inches.of( + config.physicalCharacteristics.conversionFactor.drive.diameter / + 2), + KilogramSquareMeters.of(0.02), + config.physicalCharacteristics.wheelGripCoefficientOfFriction) ); mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); @@ -332,6 +334,16 @@ public class SwerveDrive HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL); } + @Override + public void close() { + imu.close(); + tunerXRecommendation.close(); + + for (var module : swerveModules) { + module.close(); + } + } + /** * Update the cache validity period for the robot. * @@ -567,7 +579,7 @@ public class SwerveDrive if (fieldRelative) { - ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); } drive(velocity, isOpenLoop, new Translation2d()); } @@ -619,31 +631,46 @@ public class SwerveDrive } /** - * Set the maximum speeds for desaturation. + * Set the maximum attainable speeds for desaturation. * * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while * translating in meters per second. * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in * radians per second. */ - public void setMaximumSpeeds( + public void setMaximumAttainableSpeeds( double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) { this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond; this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond; - this.swerveController.config.maxAngularVelocity = attainableMaxRotationalVelocityRadiansPerSecond; + } + + /** + * Set the maximum allowable speeds for desaturation. + * + * @param maxTranslationalSpeedMetersPerSecond The allowable max speed that your robot should reach while translating + * in meters per second. + * @param maxRotationalVelocityRadiansPerSecond The allowable max speed the robot should reach while rotating in + * radians per second. + */ + public void setMaximumAllowableSpeeds( + double maxTranslationalSpeedMetersPerSecond, + double maxRotationalVelocityRadiansPerSecond) + { + this.maxChassisSpeedMPS = maxTranslationalSpeedMetersPerSecond; + this.swerveController.config.maxAngularVelocity = maxRotationalVelocityRadiansPerSecond; } /** * Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or - * {@link SwerveDrive#maxChassisSpeedMPS} whichever is higher. + * {@link SwerveDrive#maxChassisSpeedMPS} whichever is the lower limit on the robot's speed. * - * @return Maximum speed in meters/second. + * @return Minimum speed in meters/second of physically attainable and user allowable limits. */ public double getMaximumChassisVelocity() { - return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS); + return Math.min(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS); } /** @@ -651,9 +678,9 @@ public class SwerveDrive * * @return {@link LinearVelocity} representing the maximum drive speed of a module. */ - public LinearVelocity getMaximumModuleDriveVelocity() + public double getMaximumModuleDriveVelocity() { - return swerveModules[0].getMaxVelocity(); + return swerveModules[0].getMaxDriveVelocityMetersPerSecond(); } /** @@ -668,13 +695,13 @@ public class SwerveDrive /** * Get the maximum angular velocity, either {@link SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or - * {@link SwerveControllerConfiguration#maxAngularVelocity}. + * {@link SwerveControllerConfiguration#maxAngularVelocity}, whichever is the lower limit on the robot's speed. * - * @return Maximum angular velocity in radians per second. + * @return Minimum angular velocity in radians per second of physically attainable and user allowable limits. */ public double getMaximumChassisAngularVelocity() { - return Math.max(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity); + return Math.min(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity); } /** @@ -688,8 +715,9 @@ public class SwerveDrive boolean isOpenLoop) { // Desaturates wheel speeds - double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond); - if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) + double maxModuleSpeedMPS = getMaximumModuleDriveVelocity(); + if ((attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) && + attainableMaxTranslationalSpeedMetersPerSecond != maxChassisSpeedMPS) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed, maxModuleSpeedMPS, @@ -719,7 +747,7 @@ public class SwerveDrive public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) { SwerveDriveTelemetry.startCtrlCycle(); - double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond); + double maxModuleSpeedMPS = getMaximumModuleDriveVelocity(); desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS); @@ -748,6 +776,9 @@ public class SwerveDrive } for (SwerveModule module : swerveModules) { + module.applyStateOptimizations(states[module.moduleNumber]); + module.applyAntiJitter(states[module.moduleNumber], false); + // from the module configuration, obtain necessary information to calculate feed-forward // Warning: Will not work well if motor is not what we are expecting. // Warning: Should replace module.getDriveMotor().simMotor with expected motor type first. @@ -1128,7 +1159,7 @@ public class SwerveDrive { SwerveDriveTelemetry.startOdomCycle(); odometryLock.lock(); - invalidateCache(); +// invalidateCache(); try { // Update odometry @@ -1255,7 +1286,7 @@ public class SwerveDrive * {@link Timer#getFPGATimestamp()} or similar sources. * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, - * for best accuracy calculate the standard deviation at 2 or more points and fit a + * for best accuracy calculate the standard deviation at 2 or more points and fit a * line to it with the calculated optimal standard deviation. (Units should be meters * per pixel). By optimizing this you can get * vision accurate to inches instead of * feet. @@ -1355,10 +1386,34 @@ public class SwerveDrive } } + /** + * Set the motor controller closed loop feedback device to the defined external absolute encoder, with the given + * offset from the supplied configuration, overwriting any native offset. + */ + public void useExternalFeedbackSensor() + { + for (SwerveModule module : swerveModules) + { + module.useExternalFeedbackSensor(); + } + } + + /** + * Set the motor controller closed loop feedback device to the internal encoder instead of the absolute encoder. + */ + public void useInternalFeedbackSensor() + { + for (SwerveModule module : swerveModules) + { + module.useInternalFeedbackSensor(); + } + } + /** * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the * internal offsets to prevent double offsetting. */ + @Deprecated public void pushOffsetsToEncoders() { for (SwerveModule module : swerveModules) @@ -1370,6 +1425,7 @@ public class SwerveDrive /** * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 */ + @Deprecated public void restoreInternalOffset() { for (SwerveModule module : swerveModules) @@ -1378,6 +1434,20 @@ public class SwerveDrive } } + /** + * Set module optimization to be utilized or not. Sometimes it is desirable to be enabled for debugging purposes + * only. + * + * @param enabled Optimization enabled state. + */ + public void setModuleStateOptimization(boolean enabled) + { + for (SwerveModule module : swerveModules) + { + module.setModuleStateOptimization(enabled); + } + } + /** * Enable auto-centering module wheels. This has a side effect of causing some jitter to the robot when a PID is not * tuned perfectly. This function is a wrapper for {@link SwerveModule#setAntiJitter(boolean)} to perform diff --git a/swervelib/SwerveDriveTest.java b/swervelib/SwerveDriveTest.java index 70405a3..30b0a65 100644 --- a/swervelib/SwerveDriveTest.java +++ b/swervelib/SwerveDriveTest.java @@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutDistance; @@ -27,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.telemetry.SwerveDriveTelemetry; /** * Class to perform tests on the swerve drive. @@ -101,7 +104,7 @@ public class SwerveDriveTest * Power the drive motors for the swerve drive to a set voltage. * * @param swerveDrive {@link SwerveDrive} to control. - * @param volts DutyCycle percentage of voltage to send to drive motors. + * @param volts Voltage to send to drive motors. */ public static void powerDriveMotorsVoltage(SwerveDrive swerveDrive, double volts) { @@ -135,6 +138,60 @@ public class SwerveDriveTest angleModules(swerveDrive, Rotation2d.fromDegrees(0)); } + /** + * Set the modules to their rotary position to allow running sysid and spinning the robot + * + * @param swerveDrive Swerve Drive to control. + */ + public static void setModulesToRotaryPosition(SwerveDrive swerveDrive) + { + SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1)); + for (int i = 0; i < swerveDrive.getModules().length; i++) + { + swerveDrive.getModules()[i].getAngleMotor().setReference(rotaryStates[i].angle.getDegrees(), 0); + } + } + + /** + * Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent + * to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real + * robot. + * + * @param swerveDrive {@link SwerveDrive} to control. + * @param volts Voltage to send to drive motors. + * @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to + * make the robot spin, false to make the robot drive in straight line + */ + public static void runDriveMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts, + boolean testWithSpinning) + { + SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1)); + for (int i = 0; i < swerveDrive.getModules().length; i++) + { + swerveDrive.getModules()[i].getSimModule().runDriveMotorCharacterization( + testWithSpinning + ? rotaryStates[i].angle + : Rotation2d.kZero, + volts); + } + } + + /** + * Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent + * to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real + * robot. + * + * @param swerveDrive {@link SwerveDrive} to control. + * @param volts Voltage to send to angle motors. + */ + public static void runAngleMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts) + { + for (SwerveModule module : swerveDrive.getModules()) + { + module.getSimModule().runAngleMotorCharacterization(volts); + } + } + /** * Find the minimum amount of power required to move the swerve drive motors. * @@ -299,19 +356,35 @@ public class SwerveDriveTest /** * Sets up the SysId runner and logger for the drive motors * - * @param config - The SysIdRoutine.Config to use - * @param swerveSubsystem - the subsystem to add to requirements - * @param swerveDrive - the SwerveDrive from which to access motor info - * @param maxVolts - The maximum voltage that should be applied to the drive motors. + * @param config - The SysIdRoutine.Config to use + * @param swerveSubsystem - the subsystem to add to requirements + * @param swerveDrive - the SwerveDrive from which to access motor info + * @param maxVolts - The maximum voltage that should be applied to the drive motors. + * @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to + * make the robot spin, false to make the robot drive in straight line * @return A SysIdRoutine runner */ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swerveSubsystem, - SwerveDrive swerveDrive, double maxVolts) + SwerveDrive swerveDrive, double maxVolts, boolean testWithSpinning) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( (Voltage voltage) -> { - SwerveDriveTest.centerModules(swerveDrive); - SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts)); + if (!SwerveDriveTelemetry.isSimulation) + { + if (testWithSpinning) + { + SwerveDriveTest.setModulesToRotaryPosition(swerveDrive); + } else + { + SwerveDriveTest.centerModules(swerveDrive); + } + SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts)); + } else + { + SwerveDriveTest.runDriveMotorsCharacterizationOnSimModules(swerveDrive, + voltage.in(Volts), + testWithSpinning); + } }, log -> { for (SwerveModule module : swerveDrive.getModules()) @@ -382,8 +455,14 @@ public class SwerveDriveTest { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( (Voltage voltage) -> { - SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts)); - SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0); + if (!SwerveDriveTelemetry.isSimulation) + { + SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts)); + SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0); + } else + { + SwerveDriveTest.runAngleMotorsCharacterizationOnSimModules(swerveDrive, voltage.in(Volts)); + } }, log -> { for (SwerveModule module : swerveDrive.getModules()) diff --git a/swervelib/SwerveInputStream.java b/swervelib/SwerveInputStream.java index 0222bf0..ba366d3 100644 --- a/swervelib/SwerveInputStream.java +++ b/swervelib/SwerveInputStream.java @@ -1,6 +1,7 @@ package swervelib; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -44,83 +45,111 @@ public class SwerveInputStream implements Supplier /** * Translation suppliers. */ - private final DoubleSupplier controllerTranslationX; + private final DoubleSupplier controllerTranslationX; /** * Translational supplier. */ - private final DoubleSupplier controllerTranslationY; + private final DoubleSupplier controllerTranslationY; /** * {@link SwerveDrive} object for transformations. */ - private final SwerveDrive swerveDrive; + private final SwerveDrive swerveDrive; /** * Rotation supplier as angular velocity. */ - private Optional controllerOmega = Optional.empty(); + private Optional controllerOmega = Optional.empty(); /** * Controller supplier as heading. */ - private Optional controllerHeadingX = Optional.empty(); + private Optional controllerHeadingX = Optional.empty(); /** * Controller supplier as heading. */ - private Optional controllerHeadingY = Optional.empty(); + private Optional controllerHeadingY = Optional.empty(); /** * 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(); + /** + * Target {@link Supplier} to drive towards when driveToPose is enabled. + */ + private Optional> driveToPose = Optional.empty(); + /** + * {@link ProfiledPIDController} for the X translation while driving to a pose. Units are m/s + */ + private Optional driveToPoseXPIDController = Optional.empty(); + /** + * {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s + */ + private Optional driveToPoseYPIDController = Optional.empty(); + /** + * {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s + */ + private Optional driveToPoseOmegaPIDController = Optional.empty(); /** * Output {@link ChassisSpeeds} based on heading while this is True. */ - private Optional headingEnabled = Optional.empty(); + private Optional headingEnabled = Optional.empty(); /** * Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY} */ - private Optional lockedHeading = Optional.empty(); + private Optional lockedHeading = Optional.empty(); /** * Output {@link ChassisSpeeds} based on aim while this is True. */ - private Optional aimEnabled = Optional.empty(); + private Optional aimEnabled = Optional.empty(); + /** + * Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}. + */ + private Optional driveToPoseEnabled = Optional.empty(); /** * Maintain current heading and drive without rotating, ideally. */ - private Optional translationOnlyEnabled = Optional.empty(); + private Optional translationOnlyEnabled = Optional.empty(); /** * Cube the translation magnitude from the controller. */ - private Optional translationCube = Optional.empty(); + private Optional translationCube = Optional.empty(); /** * Cube the angular velocity axis from the controller. */ - private Optional omegaCube = Optional.empty(); + private Optional omegaCube = Optional.empty(); /** * Robot relative oriented output expected. */ - private Optional robotRelative = Optional.empty(); + private Optional robotRelative = Optional.empty(); /** * Field oriented chassis output is relative to your current alliance. */ - private Optional allianceRelative = Optional.empty(); + private Optional allianceRelative = Optional.empty(); + /** + * Heading offset enable state. + */ + private Optional headingOffsetEnabled = Optional.empty(); + /** + * Heading offset to apply during heading based control. + */ + private Optional headingOffset = Optional.empty(); /** * {@link SwerveController} for simple control over heading. */ - private SwerveController swerveController = null; + private SwerveController swerveController = null; /** * Current {@link SwerveInputMode} to use. */ - private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY; + private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY; /** @@ -195,9 +224,14 @@ public class SwerveInputStream implements Supplier newStream.axisDeadband = axisDeadband; newStream.translationAxisScale = translationAxisScale; newStream.omegaAxisScale = omegaAxisScale; + newStream.driveToPose = driveToPose; + newStream.driveToPoseXPIDController = driveToPoseXPIDController; + newStream.driveToPoseYPIDController = driveToPoseYPIDController; + newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController; newStream.aimTarget = aimTarget; newStream.headingEnabled = headingEnabled; newStream.aimEnabled = aimEnabled; + newStream.driveToPoseEnabled = driveToPoseEnabled; newStream.currentMode = currentMode; newStream.translationOnlyEnabled = translationOnlyEnabled; newStream.lockedHeading = lockedHeading; @@ -206,6 +240,8 @@ public class SwerveInputStream implements Supplier newStream.translationCube = translationCube; newStream.robotRelative = robotRelative; newStream.allianceRelative = allianceRelative; + newStream.headingOffsetEnabled = headingOffsetEnabled; + newStream.headingOffset = headingOffset; return newStream; } @@ -233,6 +269,103 @@ public class SwerveInputStream implements Supplier return this; } + /** + * Drive to a given pose with the provided {@link ProfiledPIDController}s + * + * @param pose {@link Supplier} for ease of use. + * @param xPIDController PID controller for the X axis, units are m/s. + * @param yPIDController PID controller for the Y axis, units are m/s. + * @param omegaPIDController PID Controller for rotational axis, units are rad/s. + * @return self + */ + public SwerveInputStream driveToPose(Supplier pose, ProfiledPIDController xPIDController, + ProfiledPIDController yPIDController, ProfiledPIDController omegaPIDController) + { + driveToPose = Optional.of(pose); + driveToPoseXPIDController = Optional.of(xPIDController); + driveToPoseYPIDController = Optional.of(yPIDController); + driveToPoseOmegaPIDController = Optional.of(omegaPIDController); + return this; + } + + /** + * Drive to a given pose with the provided {@link ProfiledPIDController}s + * + * @param pose {@link Supplier} for ease of use. + * @param translation PID controller for the X and Y axis, units are m/s. + * @param rotation PID Controller for rotational axis, units are rad/s. + * @return self + */ + public SwerveInputStream driveToPose(Supplier pose, ProfiledPIDController translation, + ProfiledPIDController rotation) + { + return driveToPose(pose, translation, new ProfiledPIDController(translation.getP(), + translation.getI(), + translation.getD(), + translation.getConstraints()), rotation); + } + + /** + * Enable driving to the target pose. + * + * @param enabled Enable state of drive to pose. + * @return self. + */ + public SwerveInputStream driveToPoseEnabled(BooleanSupplier enabled) + { + driveToPoseEnabled = Optional.of(enabled); + return this; + } + + /** + * Enable driving to the target pose. + * + * @param enabled Enable state of drive to pose. + * @return self. + */ + public SwerveInputStream driveToPoseEnabled(boolean enabled) + { + driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty(); + return this; + } + + + /** + * Heading offset enabled boolean supplier. + * + * @param enabled Enable state + * @return self + */ + public SwerveInputStream headingOffset(BooleanSupplier enabled) + { + headingOffsetEnabled = Optional.of(enabled); + return this; + } + + /** + * Heading offset enable + * + * @param enabled Enable state + * @return self + */ + public SwerveInputStream headingOffset(boolean enabled) + { + headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty(); + return this; + } + + /** + * Set the heading offset angle. + * + * @param angle {@link Rotation2d} offset to apply + * @return self + */ + public SwerveInputStream headingOffset(Rotation2d angle) + { + headingOffset = Optional.of(angle); + return this; + } + /** * Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance. * @@ -476,7 +609,19 @@ public class SwerveInputStream implements Supplier */ private SwerveInputMode findMode() { - if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean()) + if (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean()) + { + if (driveToPose.isPresent()) + { + if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() && + driveToPoseYPIDController.isPresent()) + { + return SwerveInputMode.DRIVE_TO_POSE; + } + DriverStation.reportError("Drive to pose not supplied with pid controllers.", false); + } + DriverStation.reportError("Drive to pose enabled without supplier present.", false); + } else if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean()) { return SwerveInputMode.TRANSLATION_ONLY; } else if (aimEnabled.isPresent() && aimEnabled.get().getAsBoolean()) @@ -526,17 +671,7 @@ public class SwerveInputStream implements Supplier lockedHeading = Optional.empty(); break; } - case ANGULAR_VELOCITY -> - { - // Do nothing - break; - } - case HEADING -> - { - // Do nothing - break; - } - case AIM -> + case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE -> { // Do nothing break; @@ -551,7 +686,7 @@ public class SwerveInputStream implements Supplier lockedHeading = Optional.of(swerveDrive.getOdometryHeading()); break; } - case ANGULAR_VELOCITY -> + case ANGULAR_VELOCITY, DRIVE_TO_POSE -> { if (swerveDrive.headingCorrection) { @@ -559,12 +694,7 @@ public class SwerveInputStream implements Supplier } break; } - case HEADING -> - { - // Do nothing - break; - } - case AIM -> + case HEADING, AIM -> { // Do nothing break; @@ -651,16 +781,16 @@ public class SwerveInputStream implements Supplier } /** - * Change {@link ChassisSpeeds} to robot relative. + * Change {@link ChassisSpeeds} from robot relative if enabled. * - * @param fieldRelativeSpeeds Field relative speeds to translate into robot-relative speeds. - * @return Robot relative {@link ChassisSpeeds}. + * @param fieldRelativeSpeeds Field or robot relative speeds to translate into robot-relative speeds. + * @return Field relative {@link ChassisSpeeds}. */ private ChassisSpeeds applyRobotRelativeTranslation(ChassisSpeeds fieldRelativeSpeeds) { if (robotRelative.isPresent() && robotRelative.get().getAsBoolean()) { - return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading()); + return ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading()); } return fieldRelativeSpeeds; } @@ -709,6 +839,24 @@ public class SwerveInputStream implements Supplier return fieldRelativeRotation; } + /** + * Adds offset to rotation if one is set. + * + * @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset + * @return Offsetted {@link Rotation2d} + */ + private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation) + { + if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean()) + { + if (headingOffset.isPresent()) + { + return fieldRelativeRotation.rotateBy(headingOffset.get()); + } + } + return fieldRelativeRotation; + } + /** * Gets a {@link ChassisSpeeds} * @@ -759,12 +907,22 @@ public class SwerveInputStream implements Supplier case HEADING -> { omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(), - applyAllianceAwareRotation(Rotation2d.fromRadians( - swerveController.getJoystickAngle( - controllerHeadingX.get() - .getAsDouble(), - controllerHeadingY.get() - .getAsDouble()))).getRadians()); + applyHeadingOffset( + applyAllianceAwareRotation( + Rotation2d.fromRadians( + swerveController.getJoystickAngle( + controllerHeadingX.get() + .getAsDouble(), + controllerHeadingY.get() + .getAsDouble())))).getRadians()); + + // Prevent rotation if controller heading inputs are not past axisDeadband + if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) < + axisDeadband.get()) + { + omegaRadiansPerSecond = 0; + } + speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); break; } @@ -777,6 +935,17 @@ public class SwerveInputStream implements Supplier speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); break; } + case DRIVE_TO_POSE -> + { + Pose2d target = driveToPose.get().get(); + Pose2d pose = swerveDrive.getPose(); + omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation() + .getRadians(), + target.getRotation().getRadians()); + speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()), + driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()), + omegaRadiansPerSecond); + } } currentMode = newMode; @@ -804,6 +973,10 @@ public class SwerveInputStream implements Supplier /** * Output based off of targeting. */ - AIM + AIM, + /** + * Drive to a target pose. + */ + DRIVE_TO_POSE } } diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 81bb658..0eb060e 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -1,5 +1,6 @@ package swervelib; +import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -33,7 +34,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** * The Swerve Module class which represents and controls Swerve Modules for the swerve drive. */ -public class SwerveModule +public class SwerveModule implements AutoCloseable { /** @@ -72,6 +73,29 @@ public class SwerveModule * An {@link Alert} for if there is no Absolute Encoder on the module. */ private final Alert noEncoderWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert externalSensorIsNull = new Alert("No absolute Encoder found.", + AlertType.kError); + /** + * An {@link Alert} for if the offset is 0 degrees. + */ + private final Alert internalOffsetIsZero = new Alert( + "Absolute encoder offset is 0, this may be a problem.", + AlertType.kWarning); + /** + * An {@link Alert} for if the angle/steer/azimuth motor is incompatible with the absolute encoder. + */ + private final Alert externalFeedbackIncompatible = new Alert( + "Absolute encoder is incompatible, cannot set as an external feedback device.", + AlertType.kError); + /** + * An {@link Alert} for if the absolute encoder cannot set an offset. + */ + private final Alert externalOffsetIncompatible = new Alert( + "Absolute encoder is incompatible, cannot set an offset internally.", + AlertType.kError); /** * NT4 Raw Absolute Angle publisher for the absolute encoder. */ @@ -108,6 +132,10 @@ public class SwerveModule * Maximum {@link LinearVelocity} for the drive motor of the swerve module. */ private LinearVelocity maxDriveVelocity; + /** + * Maximum velocity for the drive motor of the swerve module. + */ + private double maxDriveVelocityMetersPerSecond; /** * Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module. */ @@ -119,7 +147,7 @@ public class SwerveModule /** * Anti-Jitter AKA auto-centering disabled. */ - private boolean antiJitterEnabled = true; + private boolean antiJitterEnabled = true; /** * Last swerve module state applied. */ @@ -132,18 +160,22 @@ public class SwerveModule * Simulated swerve module. */ private SwerveModuleSimulation simModule; + /** + * Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)} + */ + private boolean optimizeSwerveModuleState = true; /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; /** * Encoder, Absolute encoder synchronization enabled. */ - private boolean synchronizeEncoderEnabled = false; + private boolean synchronizeEncoderEnabled = false; /** * Encoder synchronization deadband in degrees. */ - private double synchronizeEncoderDeadband = 3; + private double synchronizeEncoderDeadband = 3; /** @@ -196,7 +228,10 @@ public class SwerveModule absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20); // Config angle motor/controller - angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); + if (!angleMotor.usingExternalFeedbackSensor()) + { + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); + } angleMotor.configurePIDF(moduleConfiguration.anglePIDF); angleMotor.configurePIDWrapping(0, 360); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); @@ -255,6 +290,14 @@ public class SwerveModule "swerve/modules/" + configuration.name + "/Angle Setpoint").publish(); } + @Override + public void close() + { + angleMotor.close(); + driveMotor.close(); + absoluteEncoder.close(); + } + /** * Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor. * @@ -269,6 +312,31 @@ public class SwerveModule configuration.physicalCharacteristics.wheelGripCoefficientOfFriction); } + /** + * Set utilization of {@link SwerveModuleState#optimize(Rotation2d)} which should be disabled for some debugging. + * + * @param optimizationState Optimization enabled. + */ + public void setModuleStateOptimization(boolean optimizationState) + { + optimizeSwerveModuleState = optimizationState; + if (!optimizeSwerveModuleState) + { + angleMotor.disablePIDWrapping(); + angleMotor.burnFlash(); + } + } + + /** + * Check if the module state optimization used by {@link SwerveModuleState#optimize(Rotation2d)} is enabled. + * + * @return optimization state. + */ + public boolean getModuleStateOptimization() + { + return optimizeSwerveModuleState; + } + /** * Set the voltage compensation for the swerve module motor. * @@ -406,27 +474,20 @@ public class SwerveModule */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - - desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition())); - - // If we are forcing the angle - if (!force && antiJitterEnabled) - { - // Prevents module rotation if speed is less than 1% - SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocity.in(MetersPerSecond), 4)); - } + applyStateOptimizations(desiredState); + applyAntiJitter(desiredState, force); // Cosine compensation. - LinearVelocity nextVelocity = configuration.useCosineCompensator - ? getCosineCompensatedVelocity(desiredState) - : MetersPerSecond.of(desiredState.speedMetersPerSecond); - LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond); - desiredState.speedMetersPerSecond = nextVelocity.magnitude(); + double nextVelocityMetersPerSecond = configuration.useCosineCompensator + ? getCosineCompensatedVelocity(desiredState) + : desiredState.speedMetersPerSecond; + double curVelocityMetersPerSecond = lastState.speedMetersPerSecond; + desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond; setDesiredState(desiredState, isOpenLoop, - driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond), - nextVelocity.in(MetersPerSecond))); + driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond, + nextVelocityMetersPerSecond)); } /** @@ -440,7 +501,6 @@ public class SwerveModule public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, double driveFeedforwardVoltage) { - if (isOpenLoop) { double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond); @@ -497,7 +557,7 @@ public class SwerveModule * @param desiredState Desired {@link SwerveModuleState} to use. * @return Cosine compensated velocity in meters/second. */ - private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState) + private double getCosineCompensatedVelocity(SwerveModuleState desiredState) { double cosineScalar = 1.0; // Taken from the CTRE SwerveModule class. @@ -515,7 +575,39 @@ public class SwerveModule cosineScalar = 1; } - return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar); + return desiredState.speedMetersPerSecond * cosineScalar; + } + + /** + * Apply the {@link SwerveModuleState#optimize(Rotation2d)} function if the module state optimization is enabled while + * debugging. + * + * @param desiredState The desired state to apply the optimization to. + */ + public void applyStateOptimizations(SwerveModuleState desiredState) + { + // SwerveModuleState optimization might be desired to be disabled while debugging. + if (optimizeSwerveModuleState) + { + desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition())); + } + } + + /** + * Apply anti-jitter to the desired state. This will prevent the module from rotating if the speed requested is too + * low. If force is true, the anti-jitter will not be applied. + * + * @param desiredState The desired state to apply the anti-jitter to. + * @param force Whether to ignore the {@link SwerveModule#antiJitterEnabled} state and apply the anti-jitter + * anyway. + */ + public void applyAntiJitter(SwerveModuleState desiredState, boolean force) + { + if (!force && antiJitterEnabled) + { + // Prevents module rotation if speed is less than 1% + SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocityMetersPerSecond, 4)); + } } /** @@ -605,10 +697,13 @@ public class SwerveModule { angle = getRelativePosition(); } - angle %= 360; - if (angle < 0.0) + if (optimizeSwerveModuleState) { - angle += 360; + angle %= 360; + if (angle < 0.0) + { + angle += 360; + } } return angle; @@ -696,9 +791,64 @@ public class SwerveModule return configuration; } + + /** + * Use external sensors for the feedback of the angle/azimuth/steer controller. + */ + public void useExternalFeedbackSensor() + { + if (absoluteEncoder == null) + { + externalSensorIsNull.set(true); + return; + } + if (angleOffset == 0) + { + internalOffsetIsZero.set(true); + } + if (absoluteEncoder.setAbsoluteEncoderOffset(configuration.angleOffset)) + { + angleMotor.setAbsoluteEncoder(absoluteEncoder); + if (angleMotor.usingExternalFeedbackSensor()) + { + angleOffset = 0; + } else + { + externalFeedbackIncompatible.set(true); + angleMotor.setAbsoluteEncoder(null); + absoluteEncoder.setAbsoluteEncoderOffset(0); + } + + } else + { + externalOffsetIncompatible.set(true); + absoluteEncoder.setAbsoluteEncoderOffset(0); + } + } + + /** + * Use external sensors for the feedback of the angle/azimuth/steer controller. + */ + public void useInternalFeedbackSensor() + { + if (absoluteEncoder == null) + { + externalSensorIsNull.set(true); + return; + } + if (angleOffset == 0) + { + internalOffsetIsZero.set(true); + } + angleMotor.setAbsoluteEncoder(null); + absoluteEncoder.setAbsoluteEncoderOffset(0); + angleOffset = configuration.angleOffset; + } + /** * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. */ + @Deprecated public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) @@ -759,15 +909,27 @@ public class SwerveModule * @return {@link LinearVelocity} max velocity of the drive wheel. */ public LinearVelocity getMaxVelocity() + { + getMaxDriveVelocityMetersPerSecond(); + return maxDriveVelocity; + } + + /** + * Get the maximum drive velocity of the module in Meters Per Second. + * + * @return Maximum drive motor velocity in Meters Per Second. + */ + public double getMaxDriveVelocityMetersPerSecond() { if (maxDriveVelocity == null) { - maxDriveVelocity = MetersPerSecond.of( - (RadiansPerSecond.of(driveMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) / + maxDriveVelocity = InchesPerSecond.of( + (driveMotor.getSimMotor().freeSpeedRadPerSec / configuration.conversionFactors.drive.gearRatio) * - configuration.conversionFactors.drive.diameter); + configuration.conversionFactors.drive.diameter / 2.0); + maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond); } - return maxDriveVelocity; + return maxDriveVelocityMetersPerSecond; } /** @@ -780,7 +942,7 @@ public class SwerveModule if (maxAngularVelocity == null) { maxAngularVelocity = RotationsPerSecond.of( - RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) * + RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) / configuration.conversionFactors.angle.gearRatio); } return maxAngularVelocity; @@ -795,11 +957,26 @@ public class SwerveModule { rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition()); } - rawAnglePublisher.set(angleMotor.getPosition()); - rawDriveEncoderPublisher.set(drivePositionCache.getValue()); - rawDriveVelocityPublisher.set(driveVelocityCache.getValue()); + if (SwerveDriveTelemetry.isSimulation && SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SwerveModulePosition pos = simModule.getPosition(); + SwerveModuleState state = simModule.getState(); + rawAnglePublisher.set(pos.angle.getDegrees()); + rawDriveEncoderPublisher.set(pos.distanceMeters); + rawDriveVelocityPublisher.set(state.speedMetersPerSecond); + // For code coverage + angleMotor.getPosition(); + drivePositionCache.getValue(); + driveVelocityCache.getValue(); + } else + { + rawAnglePublisher.set(angleMotor.getPosition()); + rawDriveEncoderPublisher.set(drivePositionCache.getValue()); + rawDriveVelocityPublisher.set(driveVelocityCache.getValue()); + } adjAbsoluteAnglePublisher.set(getAbsolutePosition()); absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue()); + } /** diff --git a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 11016f6..3d91c6f 100644 --- a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -47,6 +47,12 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder AlertType.kWarning); } + @Override + public void close() + { + encoder.close(); + } + /** * Construct the Encoder given the analog input channel. * diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index ba21689..782012d 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -1,6 +1,5 @@ package swervelib.encoders; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.Rotations; @@ -13,7 +12,6 @@ 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; @@ -28,7 +26,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); + public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(1).in(Seconds); /** * An {@link Alert} for if the CANCoder magnet field is less than ideal. */ @@ -114,6 +112,12 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder AlertType.kWarning); } + @Override + public void close() + { + encoder.close(); + } + /** * Reset the encoder to factory defaults. */ @@ -158,6 +162,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder { readingError = false; MagnetHealthValue strength = magnetHealth.refresh().getValue(); + angle.refresh(); magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) @@ -170,8 +175,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder readingFaulty.set(false); } - angle.refresh(); - // Taken from democat's library. // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 for (int i = 0; i < maximumRetries; i++) @@ -190,8 +193,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder { readingIgnored.set(false); } - - return angle.getValue().in(Degrees); + // Convert from Rotations to Degrees. + return angle.getValueAsDouble() * 360; } /** diff --git a/swervelib/encoders/CanAndMagSwerve.java b/swervelib/encoders/CanAndMagSwerve.java index 4297158..5ed9e90 100644 --- a/swervelib/encoders/CanAndMagSwerve.java +++ b/swervelib/encoders/CanAndMagSwerve.java @@ -26,7 +26,13 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder public CanAndMagSwerve(int canid) { encoder = new Canandmag(canid); - settings = encoder.getSettings(); + settings = new CanandmagSettings(); + } + + @Override + public void close() + { + encoder.close(); } /** diff --git a/swervelib/encoders/DIODutyCycleEncoderSwerve.java b/swervelib/encoders/DIODutyCycleEncoderSwerve.java new file mode 100644 index 0000000..f8841bc --- /dev/null +++ b/swervelib/encoders/DIODutyCycleEncoderSwerve.java @@ -0,0 +1,129 @@ +package swervelib.encoders; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.Timer; + +/** + * DutyCycle encoders such as "US Digital MA3 with DIO Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag + * Encoder." attached via a DIO lane. + *

+ * Credits to + * + * p2reneker25 for building this. + */ +public class DIODutyCycleEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * Duty Cycle Encoder. + */ + private final DutyCycleEncoder encoder; + /** + * Inversion state. + */ + private boolean isInverted; + /** + * An {@link Alert} for if the encoder cannot report accurate velocities. + */ + private Alert inaccurateVelocities; + /** + * The Offset in degrees of the DIO absolute encoder. + */ + private double offset; + + /** + * Constructor for the DIO duty cycle encoder. + * + * @param pin DIO lane for the encoder. + */ + public DIODutyCycleEncoderSwerve(int pin) + { + encoder = new DutyCycleEncoder(pin); + Timer.delay(2); + inaccurateVelocities = new Alert( + "Encoders", + "The DIO Duty Cycle encoder may not report accurate velocities!", + AlertType.kWarning); + + } + + @Override + public void close() + { + encoder.close(); + } + + /** + * Configure the inversion state of the encoder. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + isInverted = inverted; + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return (isInverted ? -1.0 : 1.0) * ((encoder.get() * 360) - offset); + } + + /** + * Get the encoder object. + * + * @return {@link DutyCycleEncoder} from the class. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + inaccurateVelocities.set(true); + return encoder.get(); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + this.offset = offset; + + return true; + } +} \ No newline at end of file diff --git a/swervelib/encoders/SparkFlexEncoderSwerve.java b/swervelib/encoders/SparkFlexEncoderSwerve.java new file mode 100644 index 0000000..1d06f4b --- /dev/null +++ b/swervelib/encoders/SparkFlexEncoderSwerve.java @@ -0,0 +1,165 @@ +package swervelib.encoders; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import swervelib.motors.SparkFlexSwerve; +import swervelib.motors.SwerveMotor; + +/** + * SparkFlex absolute encoder, attached through the data port. + */ +public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder +{ + + /** + * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex. + */ + public SparkAbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * {@link SparkFlexSwerve} instance. + */ + private SwerveMotor sparkFlex; + + /** + * Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} motor. + * + * @param motor Motor to create the encoder from. + * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. + */ + public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor) + { + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkFlex Absolute Encoder", + AlertType.kWarning); + if (motor.getMotor() instanceof SparkFlex) + { + sparkFlex = motor; + encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder(); + setConversionFactor(conversionFactor); + } else + { + throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex"); + } + } + + @Override + public void close() + { + // SPARK Flex encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkFlex.close(); + } + + /** + * Reset the encoder to factory defaults. + */ + @Override + public void factoryDefault() + { + // Do nothing + } + + /** + * Clear sticky faults on the encoder. + */ + @Override + public void clearStickyFaults() + { + // Do nothing + } + + /** + * Configure the absolute encoder to read from [0, 360) per second. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + if (sparkFlex instanceof SparkFlexSwerve) + { + SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.inverted(inverted); + ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); + } + } + + /** + * Set the conversion factor of the {@link SparkFlexEncoderSwerve}. + * + * @param conversionFactor Position conversion factor from ticks to unit. + */ + public void setConversionFactor(double conversionFactor) + { + SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); + cfg.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20); + cfg.absoluteEncoder + .positionConversionFactor(conversionFactor) + .velocityConversionFactor(conversionFactor / 60); + ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); + } + + /** + * Get the absolute position of the encoder. + * + * @return Absolute position in degrees from [0, 360). + */ + @Override + public double getAbsolutePosition() + { + return encoder.getPosition(); + } + + /** + * Get the instantiated absolute encoder Object. + * + * @return Absolute encoder object. + */ + @Override + public Object getAbsoluteEncoder() + { + return encoder; + } + + /** + * Sets the Absolute Encoder Offset inside of the SparkFlex's Memory. + * + * @param offset the offset the Absolute Encoder uses as the zero point. + * @return if setting Absolute Encoder Offset was successful or not. + */ + @Override + public boolean setAbsoluteEncoderOffset(double offset) + { + if (sparkFlex instanceof SparkFlexSwerve) + { + SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + ((SparkFlexSwerve) sparkFlex).updateConfig(cfg); + return true; + } + return false; + } + + /** + * Get the velocity in degrees/sec. + * + * @return velocity in degrees/sec. + */ + @Override + public double getVelocity() + { + return encoder.getVelocity(); + } +} diff --git a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index aa88efc..bda6303 100644 --- a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -3,6 +3,7 @@ package swervelib.encoders; import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAnalogSensor; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -47,8 +48,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { sparkMax = motor; encoder = ((SparkMax) motor.getMotor()).getAnalog(); - motor.setAbsoluteEncoder(this); - sparkMax.configureIntegratedEncoder(360 / maxVoltage); + setConversionFactor(360.0 / maxVoltage); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); @@ -64,6 +64,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder } + @Override + public void close() + { + // SPARK MAX Analog encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkMax.close(); + } + /** * Run the configuration until it succeeds or times out. * @@ -81,6 +90,49 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder failureConfiguring.set(true); } + /** + * Set the conversion factor of the {@link SparkMaxAnalogEncoderSwerve}. + * + * @param conversionFactor Position conversion factor from ticks to unit. + */ + public void setConversionFactor(double conversionFactor) + { + SparkMaxConfig cfg = null; + if (sparkMax instanceof SparkMaxSwerve) + { + cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + } + if (cfg != null) + { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor); + + cfg.signals + .analogVelocityAlwaysOn(true) + .analogVoltageAlwaysOn(true) + .analogPositionAlwaysOn(true) + .analogVoltagePeriodMs(20) + .analogPositionPeriodMs(20) + .analogVelocityPeriodMs(20); + + cfg.analogSensor + .positionConversionFactor(conversionFactor) + .velocityConversionFactor(conversionFactor / 60); + } + if (sparkMax instanceof SparkMaxSwerve) + { + ((SparkMaxSwerve) sparkMax).updateConfig(cfg); + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + } + + } + + /** * Reset the encoder to factory defaults. */ @@ -110,12 +162,12 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.analogSensor.inverted(inverted); ((SparkMaxSwerve) sparkMax).updateConfig(cfg); } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) { SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.analogSensor.inverted(inverted); ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); } } diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java index b01a39f..b9f6081 100644 --- a/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,6 +4,7 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -46,7 +47,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder { failureConfiguring = new Alert( "Encoders", - "Failure configuring SparkMax Analog Encoder", + "Failure configuring SparkMax Absolute Encoder", AlertType.kWarning); offsetFailure = new Alert( "Encoders", @@ -56,14 +57,22 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder { sparkMax = motor; encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder(); - motor.setAbsoluteEncoder(this); - motor.configureIntegratedEncoder(conversionFactor); + setConversionFactor(conversionFactor); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); } } + @Override + public void close() + { + // SPARK MAX encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkFlex.close(); + } + /** * Run the configuration until it succeeds or times out. * @@ -110,16 +119,62 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.absoluteEncoder.inverted(inverted); ((SparkMaxSwerve) sparkMax).updateConfig(cfg); } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) { SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.analogSensor.inverted(true); + cfg.absoluteEncoder.inverted(inverted); ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); } } + + /** + * Set the conversion factor of the {@link SparkMaxEncoderSwerve}. + * + * @param conversionFactor Position conversion factor from ticks to unit. + */ + public void setConversionFactor(double conversionFactor) + { + // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 + // This needs to be set to 20ms or under to properly update the swerve module position for odometry + // Configuration taken from 3005, the team who helped develop the Max Swerve: + // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 + // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. + // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, + // with limited testing 19ms did not return the same value while the module was constatntly rotating. + + SparkMaxConfig cfg = null; + if (sparkMax instanceof SparkMaxSwerve) + { + cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + } + if (cfg != null) + { + cfg.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20); + + cfg.absoluteEncoder + .positionConversionFactor(conversionFactor) + .velocityConversionFactor(conversionFactor / 60); + } + if (sparkMax instanceof SparkMaxSwerve) + { + ((SparkMaxSwerve) sparkMax).updateConfig(cfg); + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + } + + } + + /** * Get the absolute position of the encoder. * @@ -151,19 +206,6 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder @Override public boolean setAbsoluteEncoderOffset(double offset) { - if (sparkMax instanceof SparkMaxSwerve) - { - SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); - cfg.absoluteEncoder.zeroOffset(offset); - ((SparkMaxSwerve) sparkMax).updateConfig(cfg); - return true; - } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) - { - SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); - cfg.absoluteEncoder.zeroOffset(offset); - ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); - return true; - } return false; } diff --git a/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java index c398c74..e89439a 100644 --- a/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -3,9 +3,15 @@ package swervelib.encoders; /** * Swerve abstraction class to define a standard interface with absolute encoders for swerve modules.. */ -public abstract class SwerveAbsoluteEncoder +public abstract class SwerveAbsoluteEncoder implements AutoCloseable { + // This is a bit weird because some encoders are closable + // while some get closed with the motor controller + // so for some encoders this will be an empty function + @Override + public abstract void close(); + /** * The maximum amount of times the swerve encoder will attempt to configure itself if failures occur. */ diff --git a/swervelib/encoders/TalonSRXEncoderSwerve.java b/swervelib/encoders/TalonSRXEncoderSwerve.java index d2449ed..aa4271b 100644 --- a/swervelib/encoders/TalonSRXEncoderSwerve.java +++ b/swervelib/encoders/TalonSRXEncoderSwerve.java @@ -44,6 +44,15 @@ public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder } } + @Override + public void close() + { + // TalonSRX encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkFlex.close(); + } + @Override public void factoryDefault() { diff --git a/swervelib/encoders/ThriftyNovaEncoderSwerve.java b/swervelib/encoders/ThriftyNovaEncoderSwerve.java index 36292a8..cacb1a3 100644 --- a/swervelib/encoders/ThriftyNovaEncoderSwerve.java +++ b/swervelib/encoders/ThriftyNovaEncoderSwerve.java @@ -6,89 +6,111 @@ 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); - } +public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder +{ - /** - * Set factory default. - */ - @Override - public void factoryDefault() { - } + /** + * 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; - /** - * Clear sticky faults. - */ - @Override - public void clearStickyFaults() { - } + /** + * 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); + } - /** - * Configure the absolute encoder. - * - * @param inverted Whether the encoder is inverted. - */ - @Override - public void configure(boolean inverted) { - this.inverted = inverted; - } + @Override + public void close() + { + // ThriftyNova encoder gets closed with the motor + // I don't think an encoder getting closed should + // close the entire motor so i will keep this empty + // sparkFlex.close(); + } - /** - * 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); - } + /** + * Set factory default. + */ + @Override + public void factoryDefault() + { + } - /** - * Get the instantiated absolute encoder Object. - */ - @Override - public Object getAbsoluteEncoder() { - return null; - } + /** + * Clear sticky faults. + */ + @Override + public void clearStickyFaults() + { + } - /** - * 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; - } + /** + * Configure the absolute encoder. + * + * @param inverted Whether the encoder is inverted. + */ + @Override + public void configure(boolean inverted) + { + this.inverted = inverted; + } - /** - * 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(); - } + /** + * 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 fc5f636..fcea482 100644 --- a/swervelib/imu/ADIS16448Swerve.java +++ b/swervelib/imu/ADIS16448Swerve.java @@ -20,7 +20,7 @@ public class ADIS16448Swerve extends SwerveIMU */ private final ADIS16448_IMU imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -42,6 +42,11 @@ public class ADIS16448Swerve extends SwerveIMU SmartDashboard.putData(imu); } + @Override + public void close() { + imu.close(); + } + /** * Reset IMU to factory default. */ diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java index 79603e0..0da8211 100644 --- a/swervelib/imu/ADIS16470Swerve.java +++ b/swervelib/imu/ADIS16470Swerve.java @@ -21,7 +21,7 @@ public class ADIS16470Swerve extends SwerveIMU */ private final ADIS16470_IMU imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -44,6 +44,11 @@ public class ADIS16470Swerve extends SwerveIMU SmartDashboard.putData(imu); } + @Override + public void close() { + imu.close(); + } + /** * Reset IMU to factory default. */ diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java index d2be03f..85c7523 100644 --- a/swervelib/imu/ADXRS450Swerve.java +++ b/swervelib/imu/ADXRS450Swerve.java @@ -20,7 +20,7 @@ public class ADXRS450Swerve extends SwerveIMU */ private final ADXRS450_Gyro imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -42,6 +42,11 @@ public class ADXRS450Swerve extends SwerveIMU SmartDashboard.putData(imu); } + @Override + public void close() { + imu.close(); + } + /** * Reset IMU to factory default. */ diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java index 168c365..b5450f5 100644 --- a/swervelib/imu/AnalogGyroSwerve.java +++ b/swervelib/imu/AnalogGyroSwerve.java @@ -20,7 +20,7 @@ public class AnalogGyroSwerve extends SwerveIMU */ private final AnalogGyro imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -49,6 +49,11 @@ public class AnalogGyroSwerve extends SwerveIMU SmartDashboard.putData(imu); } + @Override + public void close() { + imu.close(); + } + /** * Reset IMU to factory default. */ diff --git a/swervelib/imu/CanandgyroSwerve.java b/swervelib/imu/CanandgyroSwerve.java index 357dc0c..7459d51 100644 --- a/swervelib/imu/CanandgyroSwerve.java +++ b/swervelib/imu/CanandgyroSwerve.java @@ -6,7 +6,6 @@ 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.MutAngularVelocity; - import java.util.Optional; /** @@ -24,7 +23,7 @@ public class CanandgyroSwerve extends SwerveIMU */ private final Canandgyro imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, RotationsPerSecond); /** @@ -46,6 +45,12 @@ public class CanandgyroSwerve extends SwerveIMU imu = new Canandgyro(canid); } + @Override + public void close() + { + imu.close(); + } + /** * Reset {@link Canandgyro} to factory default. */ diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index 64f97d0..b0f837b 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -18,7 +18,7 @@ public class NavXSwerve extends SwerveIMU { /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -60,6 +60,10 @@ public class NavXSwerve extends SwerveIMU } } + @Override + public void close() { + imu.close(); + } /** * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index 752a3b9..7d4af3a 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -23,41 +23,41 @@ public class Pigeon2Swerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** * {@link Pigeon2} IMU device. */ - private final Pigeon2 imu; + private final Pigeon2 imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ - private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); - - /** - * Offset for the {@link Pigeon2}. - */ - private Rotation3d offset = new Rotation3d(); - /** - * Inversion for the gyro - */ - private boolean invertedIMU = false; - /** - * {@link Pigeon2} configurator. - */ - private Pigeon2Configurator cfg; - + private final MutAngularVelocity yawVel = new MutAngularVelocity(0, + 0, + DegreesPerSecond); /** * X Acceleration supplier */ - private Supplier> xAcc; + private final Supplier> xAcc; /** * Y Accelleration supplier. */ - private Supplier> yAcc; + private final Supplier> yAcc; /** * Z Acceleration supplier. */ - private Supplier> zAcc; + private final Supplier> zAcc; + /** + * Offset for the {@link Pigeon2}. + */ + private Rotation3d offset = new Rotation3d(); + /** + * Inversion for the gyro + */ + private boolean invertedIMU = false; + /** + * {@link Pigeon2} configurator. + */ + private Pigeon2Configurator cfg; /** * Generate the SwerveIMU for {@link Pigeon2}. @@ -85,6 +85,12 @@ public class Pigeon2Swerve extends SwerveIMU this(canid, ""); } + @Override + public void close() { + imu.close(); + } + + /** * Reset {@link Pigeon2} to factory default. */ @@ -159,15 +165,15 @@ public class Pigeon2Swerve extends SwerveIMU @Override public Optional getAccel() { - // TODO: Implement later. - - return Optional.empty(); + return Optional.of(new Translation3d(xAcc.get().getValueAsDouble(), + yAcc.get().getValueAsDouble(), + zAcc.get().getValueAsDouble())); } @Override public MutAngularVelocity getYawAngularVelocity() { - return yawVel.mut_replace(imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()); + return yawVel.mut_replace(imu.getAngularVelocityZWorld().refresh().getValue()); } /** diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java index 81eb2da..f6cfe3c 100644 --- a/swervelib/imu/PigeonSwerve.java +++ b/swervelib/imu/PigeonSwerve.java @@ -21,7 +21,7 @@ public class PigeonSwerve extends SwerveIMU */ private final WPI_PigeonIMU imu; /** - * Mutable {@link AngularVelocity} for readings. + * Mutable {@link MutAngularVelocity} for readings. */ private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); /** @@ -45,6 +45,12 @@ public class PigeonSwerve extends SwerveIMU SmartDashboard.putData(imu); } + @Override + public void close() { + imu.close(); + } + + /** * Reset IMU to factory default. */ diff --git a/swervelib/imu/PigeonViaTalonSRXSwerve.java b/swervelib/imu/PigeonViaTalonSRXSwerve.java new file mode 100644 index 0000000..4df05fb --- /dev/null +++ b/swervelib/imu/PigeonViaTalonSRXSwerve.java @@ -0,0 +1,158 @@ +package swervelib.imu; + +import static edu.wpi.first.units.Units.DegreesPerSecond; + +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +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; + +/** + * SwerveIMU interface for the {@link WPI_PigeonIMU}. + */ +public class PigeonViaTalonSRXSwerve extends SwerveIMU +{ + + + /** + * {@link TalonSRX} TalonSRX the IMU is attached to. + */ + private final WPI_TalonSRX talon; + + /** + * {@link WPI_PigeonIMU} IMU device. + */ + private final WPI_PigeonIMU imu; + /** + * Mutable {@link AngularVelocity} for readings. + */ + private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); + /** + * Offset for the {@link WPI_PigeonIMU}. + */ + private Rotation3d offset = new Rotation3d(); + /** + * Inversion for the gyro + */ + private boolean invertedIMU = false; + + /** + * Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}. + * + * @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus. + */ + public PigeonViaTalonSRXSwerve(int canid) + { + talon = new WPI_TalonSRX(canid); + imu = new WPI_PigeonIMU(talon); + offset = new Rotation3d(); + SmartDashboard.putData(imu); + } + + @Override + public void close() { + imu.close(); + talon.close(); + } + + /** + * Reset IMU to factory default. + */ + @Override + public void factoryDefault() + { + imu.configFactoryDefault(); + } + + /** + * Clear sticky faults on IMU. + */ + @Override + public void clearStickyFaults() + { + imu.clearStickyFaults(); + } + + /** + * Set the gyro offset. + * + * @param offset gyro offset as a {@link Rotation3d}. + */ + public void setOffset(Rotation3d offset) + { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + public void setInverted(boolean invertIMU) + { + invertedIMU = invertIMU; + } + + /** + * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRawRotation3d() + { + double[] wxyz = new double[4]; + imu.get6dQuaternion(wxyz); + Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); + return invertedIMU ? reading.unaryMinus() : reading; + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() + { + return getRawRotation3d().minus(offset); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + short[] initial = new short[3]; + imu.getBiasedAccelerometer(initial); + return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0)); + } + + @Override + public MutAngularVelocity getYawAngularVelocity() + { + return yawVel.mut_setMagnitude(imu.getRate()); + } + + /** + * Get the instantiated {@link WPI_PigeonIMU} IMU object. + * + * @return IMU object. + */ + @Override + public Object getIMU() + { + return imu; + } + +} diff --git a/swervelib/imu/SwerveIMU.java b/swervelib/imu/SwerveIMU.java index 3d58d7d..d16999b 100644 --- a/swervelib/imu/SwerveIMU.java +++ b/swervelib/imu/SwerveIMU.java @@ -3,15 +3,17 @@ package swervelib.imu; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.MutAngularVelocity; - import java.util.Optional; /** * Swerve IMU abstraction to define a standard interface with a swerve drive. */ -public abstract class SwerveIMU +public abstract class SwerveIMU implements AutoCloseable { + @Override + public abstract void close(); + /** * Reset IMU to factory default. */ diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index 7fa8f71..ed4f191 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -95,7 +95,7 @@ public class SwerveMath optimalVoltage / calculateMaxAcceleration(wheelGripCoefficientOfFriction); /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) - return new SimpleMotorFeedforward(0, kv, ka); + return new SimpleMotorFeedforward(0, kv, 0); } /** diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index eab749f..b66b934 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -6,22 +6,22 @@ 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; -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; 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.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -33,46 +33,42 @@ import swervelib.telemetry.SwerveDriveTelemetry; public class SparkFlexSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * {@link SparkFlex} Instance. */ - private final SparkFlex motor; + private final SparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkFlex (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public Optional absoluteEncoder = Optional.empty(); /** * Closed-loop PID controller. */ - public SparkClosedLoopController pid; + public SparkClosedLoopController pid; /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; - /** - * Factory default already occurred. - */ - private boolean factoryDefaultOccurred = false; + private Supplier position; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguring; - /** - * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. - */ - private Alert absoluteEncoderOffsetWarning; + private Alert failureConfiguring; /** * Configuration object for {@link SparkFlex} motor. */ - private SparkFlexConfig cfg = new SparkFlexConfig(); + private SparkFlexConfig cfg = new SparkFlexConfig(); /** @@ -86,6 +82,10 @@ public class SparkFlexSwerve extends SwerveMotor { this.motor = motor; this.isDriveMotor = isDriveMotor; + failureConfiguring = new Alert("Motors", + "Failure configuring motor " + + motor.getDeviceId(), + AlertType.kWarning); factoryDefaults(); clearStickyFaults(); @@ -95,14 +95,7 @@ public class SparkFlexSwerve extends SwerveMotor // Spin off configurations in a different thread. // configureSparkFlex(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. - failureConfiguring = new Alert("Motors", - "Failure configuring motor " + - motor.getDeviceId(), - AlertType.kWarning); - absoluteEncoderOffsetWarning = new Alert("Motors", - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + - "absoluteEncoderOffset in the Swerve Module JSON!", - AlertType.kWarning); + velocity = encoder::getVelocity; position = encoder::getPosition; } @@ -132,11 +125,17 @@ public class SparkFlexSwerve extends SwerveMotor { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } failureConfiguring.set(true); } + @Override + public void close() + { + motor.close(); + } + /** * Get the current configuration of the {@link SparkFlex} * @@ -230,9 +229,9 @@ public class SparkFlexSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -264,7 +263,7 @@ public class SparkFlexSwerve extends SwerveMotor { if (encoder == null) { - absoluteEncoder = null; + this.absoluteEncoder = Optional.empty(); cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); velocity = this.encoder::getVelocity; @@ -272,11 +271,10 @@ public class SparkFlexSwerve extends SwerveMotor } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - absoluteEncoderOffsetWarning.set(true); - absoluteEncoder = encoder; + this.absoluteEncoder = Optional.of(encoder); - velocity = absoluteEncoder::getVelocity; - position = absoluteEncoder::getAbsolutePosition; + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; } return this; } @@ -301,56 +299,31 @@ public class SparkFlexSwerve extends SwerveMotor .primaryEncoderVelocityAlwaysOn(false) .iAccumulationAlwaysOn(false) .appliedOutputPeriodMs(10) - .faultsPeriodMs(20) - ; - if (absoluteEncoder == null) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + .faultsPeriodMs(20); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); - cfg.encoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller - // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) - // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag - // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 - // This value was taken from: - // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 - // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches - cfg.encoder - .quadratureMeasurementPeriod(10) - .quadratureAverageDepth(2); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 - // Unused frames can be set to 65535 to decrease CAN ultilization. - cfg.signals - .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs(20); + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); - } else - { - // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 - // This needs to be set to 20ms or under to properly update the swerve module position for odometry - // Configuration taken from 3005, the team who helped develop the Max Swerve: - // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 - // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. - // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, - // with limited testing 19ms did not return the same value while the module was constatntly rotating. - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - - cfg.signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20); - - cfg.absoluteEncoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } - } } @@ -382,6 +355,15 @@ public class SparkFlexSwerve extends SwerveMotor } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.closedLoop.positionWrappingEnabled(false); + } + /** * Set the idle mode. * @@ -539,7 +521,7 @@ public class SparkFlexSwerve extends SwerveMotor @Override public void setPosition(double position) { - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { configureSparkFlex(() -> encoder.setPosition(position)); } diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index db3ea10..9f8889b 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -3,13 +3,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; -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; @@ -35,50 +34,50 @@ import swervelib.telemetry.SwerveDriveTelemetry; public class SparkMaxBrushedMotorSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * SparkMAX Instance. */ - private final SparkMax motor; + private final SparkMax motor; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public Optional absoluteEncoder; /** * Integrated encoder. */ - public Optional encoder = Optional.empty(); + public Optional encoder = Optional.empty(); /** * Closed-loop PID controller. */ - public SparkClosedLoopController pid; + public SparkClosedLoopController pid; /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; - /** - * Factory default already occurred. - */ - private boolean factoryDefaultOccurred = false; + private Supplier position; /** * An {@link Alert} for if the motor has no encoder. */ - private Alert noEncoderAlert; + private Alert noEncoderAlert; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguringAlert; + private Alert failureConfiguringAlert; /** * An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert; + private Alert noEncoderDefinedAlert; /** * Configuration object for {@link SparkMax} motor. */ - private SparkMaxConfig cfg = new SparkMaxConfig(); + private SparkMaxConfig cfg = new SparkMaxConfig(); /** * Initialize the swerve motor. @@ -187,11 +186,17 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } failureConfiguringAlert.set(true); } + @Override + public void close() + { + motor.close(); + } + /** * Get the current configuration of the {@link SparkMax} * @@ -284,9 +289,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -318,7 +323,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { if (encoder == null) { - absoluteEncoder = null; + this.absoluteEncoder = Optional.empty(); cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); this.encoder.ifPresentOrElse((RelativeEncoder enc) -> { @@ -333,16 +338,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); - DriverStation.reportWarning( - "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); - absoluteEncoder = encoder; - velocity = absoluteEncoder::getVelocity; - position = absoluteEncoder::getAbsolutePosition; + this.absoluteEncoder = Optional.of(encoder); + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; noEncoderDefinedAlert.set(false); } - if (absoluteEncoder == null && this.encoder.isEmpty()) + if (absoluteEncoder.isEmpty() && this.encoder.isEmpty()) { noEncoderDefinedAlert.set(true); throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); @@ -371,65 +372,29 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor .iAccumulationAlwaysOn(false) .appliedOutputPeriodMs(10) .faultsPeriodMs(20); - if (absoluteEncoder == null) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); - cfg.encoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller - // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) - // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag - // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 - // This value was taken from: - // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 - // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches - cfg.encoder - .quadratureMeasurementPeriod(10) - .quadratureAverageDepth(2); - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 - // Unused frames can be set to 65535 to decrease CAN ultilization. - cfg.signals - .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs(20); - } else - { - // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 - // This needs to be set to 20ms or under to properly update the swerve module position for odometry - // Configuration taken from 3005, the team who helped develop the Max Swerve: - // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 - // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. - // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, - // with limited testing 19ms did not return the same value while the module was constatntly rotating. - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); - cfg.signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20); - - cfg.absoluteEncoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } else - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor); - - cfg.signals - .analogVoltageAlwaysOn(true) - .analogPositionAlwaysOn(true) - .analogVoltagePeriodMs(20) - .analogPositionPeriodMs(20); - - cfg.analogSensor - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } - } + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); } /** @@ -460,6 +425,15 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.closedLoop.positionWrappingEnabled(false); + } + /** * Set the idle mode. * @@ -480,6 +454,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor public void setInverted(boolean inverted) { cfg.inverted(inverted); + if (isDriveMotor) + { + cfg.encoder.inverted(inverted); + } } /** @@ -620,7 +598,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor @Override public void setPosition(double position) { - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { encoder.ifPresent((RelativeEncoder enc) -> { configureSparkMax(() -> enc.setPosition(position)); diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index 5cf8515..8efcea8 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -3,7 +3,6 @@ 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; @@ -19,6 +18,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; import java.util.function.Supplier; import swervelib.encoders.SparkMaxAnalogEncoderSwerve; import swervelib.encoders.SparkMaxEncoderSwerve; @@ -32,38 +32,38 @@ import swervelib.telemetry.SwerveDriveTelemetry; public class SparkMaxSwerve extends SwerveMotor { + /** + * Config retry delay. + */ + private final double configDelay = Milliseconds.of(5).in(Seconds); /** * {@link SparkMax} Instance. */ - private final SparkMax motor; + private final SparkMax motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; - /** - * Absolute encoder attached to the SparkMax (if exists) - */ - public SwerveAbsoluteEncoder absoluteEncoder; + public RelativeEncoder encoder; /** * Closed-loop PID controller. */ - public SparkClosedLoopController pid; + public SparkClosedLoopController pid; /** - * Factory default already occurred. + * Absolute encoder attached to the SparkMax (if exists) */ - private boolean factoryDefaultOccurred = false; + private Optional absoluteEncoder = Optional.empty(); /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; + private Supplier position; /** * Configuration object for {@link SparkMax} motor. */ - private SparkMaxConfig cfg = new SparkMaxConfig(); + private SparkMaxConfig cfg = new SparkMaxConfig(); /** @@ -118,11 +118,17 @@ public class SparkMaxSwerve extends SwerveMotor { return; } - Timer.delay(Milliseconds.of(5).in(Seconds)); + Timer.delay(configDelay); } DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); } + @Override + public void close() + { + motor.close(); + } + /** * Get the current configuration of the {@link SparkMax} * @@ -217,9 +223,9 @@ public class SparkMaxSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { - return absoluteEncoder != null; + return absoluteEncoder.isPresent(); } /** @@ -251,7 +257,7 @@ public class SparkMaxSwerve extends SwerveMotor { if (encoder == null) { - absoluteEncoder = null; + this.absoluteEncoder = Optional.empty(); cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); velocity = this.encoder::getVelocity; @@ -262,13 +268,9 @@ public class SparkMaxSwerve extends SwerveMotor cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); - DriverStation.reportWarning( - "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); - absoluteEncoder = encoder; - velocity = absoluteEncoder::getVelocity; - position = absoluteEncoder::getAbsolutePosition; + this.absoluteEncoder = Optional.of(encoder); + velocity = this.absoluteEncoder.get()::getVelocity; + position = this.absoluteEncoder.get()::getAbsolutePosition; } return this; } @@ -295,66 +297,29 @@ public class SparkMaxSwerve extends SwerveMotor .appliedOutputPeriodMs(10) .faultsPeriodMs(20); - if (absoluteEncoder == null) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); - cfg.encoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller - // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) - // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag - // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 - // This value was taken from: - // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 - // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches - cfg.encoder - .quadratureMeasurementPeriod(10) - .quadratureAverageDepth(2); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); - // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 - // Unused frames can be set to 65535 to decrease CAN ultilization. - cfg.signals - .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs(20); + // Taken from + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); - } else - { - // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 - // This needs to be set to 20ms or under to properly update the swerve module position for odometry - // Configuration taken from 3005, the team who helped develop the Max Swerve: - // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 - // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. - // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, - // with limited testing 19ms did not return the same value while the module was constatntly rotating. - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - - cfg.signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20); - - cfg.absoluteEncoder - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } else - { - cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor); - - cfg.signals - .analogVoltageAlwaysOn(true) - .analogPositionAlwaysOn(true) - .analogVoltagePeriodMs(20) - .analogPositionPeriodMs(20); - - cfg.analogSensor - .positionConversionFactor(positionConversionFactor) - .velocityConversionFactor(positionConversionFactor / 60); - } - } } @@ -387,6 +352,16 @@ public class SparkMaxSwerve extends SwerveMotor } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.closedLoop + .positionWrappingEnabled(false); + } + /** * Set the idle mode. * @@ -546,7 +521,7 @@ public class SparkMaxSwerve extends SwerveMotor @Override public void setPosition(double position) { - if (absoluteEncoder == null) + if (absoluteEncoder.isEmpty()) { configureSparkMax(() -> encoder.setPosition(position)); } diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java index 06940c6..d9f6388 100644 --- a/swervelib/motors/SwerveMotor.java +++ b/swervelib/motors/SwerveMotor.java @@ -7,9 +7,12 @@ import swervelib.parser.PIDFConfig; /** * Swerve motor abstraction which defines a standard interface for motors within a swerve module. */ -public abstract class SwerveMotor +public abstract class SwerveMotor implements AutoCloseable { + @Override + public abstract void close(); + /** * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur. */ @@ -65,6 +68,11 @@ public abstract class SwerveMotor */ public abstract void configurePIDWrapping(double minInput, double maxInput); + /** + * Disable PID Wrapping on the motor. + */ + public abstract void disablePIDWrapping(); + /** * Set the idle mode. * @@ -191,5 +199,5 @@ public abstract class SwerveMotor * * @return connected absolute encoder state. */ - public abstract boolean isAttachedAbsoluteEncoder(); + public abstract boolean usingExternalFeedbackSensor(); } diff --git a/swervelib/motors/TalonFXSSwerve.java b/swervelib/motors/TalonFXSSwerve.java new file mode 100644 index 0000000..7a002be --- /dev/null +++ b/swervelib/motors/TalonFXSSwerve.java @@ -0,0 +1,467 @@ +package swervelib.motors; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFXS; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.system.plant.DCMotor; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.parser.PIDFConfig; +import swervelib.telemetry.SwerveDriveTelemetry; + +/** + * {@link TalonFXS} Swerve Motor. Made by Team 1466 WebbRobotics. + */ +public class TalonFXSSwerve extends SwerveMotor +{ + + /** + * Wait time for status frames to show up. + */ + public static double STATUS_TIMEOUT_SECONDS = 0.02; + /** + * Factory default already occurred. + */ + private final boolean factoryDefaultOccurred = false; + /** + * Whether the absolute encoder is integrated. + */ + private final boolean absoluteEncoder = false; + /** + * Motion magic angle voltage setter. + */ + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + /** + * Velocity voltage setter for controlling drive motor. + */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + /** + * TalonFXS motor controller. + */ + private final TalonFXS motor; + /** + * Conversion factor for the motor. + */ + private double conversionFactor; + /** + * Current TalonFXS configuration. + */ + private TalonFXSConfiguration configuration = new TalonFXSConfiguration(); + /** + * Current TalonFXS Configurator. + */ + private TalonFXSConfigurator cfg; + + + /** + * Constructor for TalonFXS swerve motor. + * + * @param motor Motor to use. + * @param isDriveMotor Whether this motor is a drive motor. + * @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to. + */ + public TalonFXSSwerve(TalonFXS motor, boolean isDriveMotor, DCMotor motorType) + { + this.isDriveMotor = isDriveMotor; + this.motor = motor; + this.cfg = motor.getConfigurator(); + this.simMotor = motorType; + + factoryDefaults(); + clearStickyFaults(); + + } + + /** + * Construct the TalonFXS swerve motor given the ID and CANBus. + * + * @param id ID of the TalonFXS on the CANBus. + * @param canbus CANBus on which the TalonFXS is on. + * @param isDriveMotor Whether the motor is a drive or steering motor. + * @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to. + */ + public TalonFXSSwerve(int id, String canbus, boolean isDriveMotor, DCMotor motorType) + { + this(new TalonFXS(id, canbus), isDriveMotor, motorType); + } + + /** + * Construct the TalonFXS swerve motor given the ID. + * + * @param id ID of the TalonFXS on the canbus. + * @param isDriveMotor Whether the motor is a drive or steering motor. + * @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to. + */ + public TalonFXSSwerve(int id, boolean isDriveMotor, DCMotor motorType) + { + this(new TalonFXS(id), isDriveMotor, motorType); + } + + /** + * Configure the factory defaults. + */ + @Override + public void factoryDefaults() + { + if (!factoryDefaultOccurred) + { + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration); + + m_angleVoltageSetter.UpdateFreqHz = 0; + // m_angleVoltageExpoSetter.UpdateFreqHz = 0; + m_velocityVoltageSetter.UpdateFreqHz = 0; + // motor.configFactoryDefault(); + // motor.setSensorPhase(true); + // motor.configSelectedFeedbackSensor(TalonFXSFeedbackDevice.IntegratedSensor, 0, 30); + // motor.configNeutralDeadband(0.001); + } + } + + /** + * Clear the sticky faults on the motor controller. + */ + @Override + public void clearStickyFaults() + { + motor.clearStickyFaults(); + } + + /** + * Set the absolute encoder to be a compatible absolute encoder. + * + * @param encoder The encoder to use. + */ + @Override + public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) + { + // Do not support. + return this; + } + + /** + * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity. + * + * @param positionConversionFactor The conversion factor to apply for position. + *


+ * Degrees:
+ * + * 360 / (angleGearRatio * encoderTicksPerRotation) + *
+ *


+ * Meters:
+ * + * (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation) + * + */ + @Override + public void configureIntegratedEncoder(double positionConversionFactor) + { + cfg.refresh(configuration); + + positionConversionFactor = 1 / positionConversionFactor; + if (!isDriveMotor) + { + positionConversionFactor *= 360; + } + conversionFactor = positionConversionFactor; + + configuration.MotionMagic = + configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor) + .withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100) + .withMotionMagicExpo_kV(0.12 * positionConversionFactor) + .withMotionMagicExpo_kA(0.1); + + /* + configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) + .withSensorToMechanismRatio(positionConversionFactor); + */ + + cfg.apply(configuration); + } + + /** + * 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) + { + + cfg.refresh(configuration.Slot0); + cfg.apply( + configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f)); + // configuration.slot0.integralZone = config.iz; + // configuration.slot0.closedLoopPeakOutput = config.output.max; + } + + /** + * 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) + { + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = true; + cfg.apply(configuration.ClosedLoopGeneral); + } + + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = false; + cfg.apply(configuration.ClosedLoopGeneral); + } + + /** + * Set the idle mode. + * + * @param isBrakeMode Set the brake mode. + */ + @Override + public void setMotorBrake(boolean isBrakeMode) + { + motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + /** + * Set the motor to be inverted. + * + * @param inverted State of inversion. + */ + @Override + public void setInverted(boolean inverted) + { + // Timer.delay(1); + cfg.refresh(configuration.MotorOutput); + configuration.MotorOutput.withInverted( + inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive); + cfg.apply(configuration.MotorOutput); + } + + /** + * 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.set(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 (SwerveDriveTelemetry.isSimulation) + // { + // PhysicsSim.getInstance().run(); + // } + + if (isDriveMotor) + { + motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward)); + } else + { + motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0)); + } + } + + /** + * Get the voltage output of the motor controller. + * + * @return Voltage output. + */ + @Override + public double getVoltage() + { + return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts); + } + + /** + * Set the voltage of the motor. + * + * @param voltage Voltage to set. + */ + @Override + public void setVoltage(double voltage) + { + motor.setVoltage(voltage); + } + + /** + * Get the applied dutycycle output. + * + * @return Applied dutycycle output to the motor. + */ + @Override + public double getAppliedOutput() + { + return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); + } + + /** + * Get the velocity of the integrated encoder. + * + * @return velocity in Meters Per Second, or Degrees per Second. + */ + @Override + public double getVelocity() + { + return motor.getVelocity().getValue().magnitude(); + } + + /** + * Get the position of the integrated encoder. + * + * @return Position in Meters or Degrees. + */ + @Override + public double getPosition() + { + return motor.getPosition().getValue().magnitude(); + } + + /** + * Set the integrated encoder position. + * + * @param position Integrated encoder position. Should be angle in degrees or meters. + */ + @Override + public void setPosition(double position) + { + if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) + { + cfg.setPosition(Degrees.of(position).in(Rotations)); + } + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param nominalVoltage Nominal voltage for operation to output to. + */ + @Override + public void setVoltageCompensation(double nominalVoltage) + { + // Do not implement + } + + /** + * 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) + { + cfg.refresh(configuration.CurrentLimits); + cfg.apply( + configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit) + .withSupplyCurrentLimitEnable(true)); + } + + /** + * 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) + { + cfg.refresh(configuration.ClosedLoopRamps); + cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate)); + } + + /** + * Get the motor object from the module. + * + * @return Motor object. + */ + @Override + public Object getMotor() + { + return motor; + } + + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + if (simMotor == null) + { + simMotor = DCMotor.getKrakenX60(1); + } + return simMotor; + } + + /** + * Queries whether the absolute encoder is directly attached to the motor controller. + * + * @return connected absolute encoder state. + */ + @Override + public boolean usingExternalFeedbackSensor() + { + return absoluteEncoder; + } + + /** + * Closes handles for unit testing. + */ + @Override + public void close() + { + motor.close(); + } +} diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index 7d3b98a..a3081a6 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -131,6 +131,12 @@ public class TalonFXSwerve extends SwerveMotor } } + @Override + public void close() { + motor.close(); + } + + /** * Clear the sticky faults on the motor controller. */ @@ -221,6 +227,17 @@ public class TalonFXSwerve extends SwerveMotor cfg.apply(configuration.ClosedLoopGeneral); } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + cfg.refresh(configuration.ClosedLoopGeneral); + configuration.ClosedLoopGeneral.ContinuousWrap = false; + cfg.apply(configuration.ClosedLoopGeneral); + } + /** * Set the idle mode. * @@ -442,7 +459,7 @@ public class TalonFXSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { return absoluteEncoder; } diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 00958e8..0126a48 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -9,6 +9,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.math.system.plant.DCMotor; import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.encoders.TalonSRXEncoderSwerve; import swervelib.math.SwerveMath; import swervelib.parser.PIDFConfig; import swervelib.parser.json.modules.ConversionFactorsJson; @@ -31,7 +32,7 @@ public class TalonSRXSwerve extends SwerveMotor /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private boolean absoluteEncoder = false; /** * TalonSRX motor controller. */ @@ -84,6 +85,11 @@ public class TalonSRXSwerve extends SwerveMotor this(new WPI_TalonSRX(id), isDriveMotor, motorType); } + @Override + public void close() { + motor.close(); + } + /** * Configure the factory defaults. */ @@ -114,6 +120,7 @@ public class TalonSRXSwerve extends SwerveMotor @Override public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { + absoluteEncoder = encoder instanceof TalonSRXEncoderSwerve; // Do not support. return this; } @@ -217,6 +224,15 @@ public class TalonSRXSwerve extends SwerveMotor // Do nothing } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + // Do nothing + } + /** * Set the idle mode. * @@ -468,7 +484,7 @@ public class TalonSRXSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { return absoluteEncoder; } diff --git a/swervelib/motors/ThriftyNovaSwerve.java b/swervelib/motors/ThriftyNovaSwerve.java index 17b6a75..3ff7f2f 100644 --- a/swervelib/motors/ThriftyNovaSwerve.java +++ b/swervelib/motors/ThriftyNovaSwerve.java @@ -26,6 +26,14 @@ import swervelib.parser.PIDFConfig; public class ThriftyNovaSwerve extends SwerveMotor { + /** + * {@link DCMotor} for simulation and calculations. + */ + private final DCMotor simMotor; + /** + * Closed-loop PID controller. + */ + public PIDController pid; /** * ThriftyNova Instance. */ @@ -34,10 +42,6 @@ public class ThriftyNovaSwerve extends SwerveMotor * The Encoder type being used */ private EncoderType encoderType; - /** - * Closed-loop PID controller. - */ - public PIDController pid; /** * Factory default already occurred. */ @@ -58,10 +62,6 @@ public class ThriftyNovaSwerve extends SwerveMotor * 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. @@ -105,6 +105,16 @@ public class ThriftyNovaSwerve extends SwerveMotor this(new ThriftyNova(id), isDriveMotor, motor); } + @Override + public void close() { + try { + motor.close(); + } catch (Exception e) { + throw new RuntimeException(e); + } + } + + /** * Set factory defaults on the motor controller. */ @@ -226,6 +236,15 @@ public class ThriftyNovaSwerve extends SwerveMotor // Do nothing } + /** + * Disable PID Wrapping on the motor. + */ + @Override + public void disablePIDWrapping() + { + // Do nothing + } + /** * Set the idle mode. * @@ -416,7 +435,7 @@ public class ThriftyNovaSwerve extends SwerveMotor * @return connected absolute encoder state. */ @Override - public boolean isAttachedAbsoluteEncoder() + public boolean usingExternalFeedbackSensor() { return EncoderType.ABS == encoderType; } diff --git a/swervelib/parser/PIDFConfig.java b/swervelib/parser/PIDFConfig.java index 9da0684..b9ec98d 100644 --- a/swervelib/parser/PIDFConfig.java +++ b/swervelib/parser/PIDFConfig.java @@ -103,6 +103,10 @@ public class PIDFConfig */ public PIDController createPIDController() { - return new PIDController(p, i, d); + PIDController pidController = new PIDController(p, i, d); + if (iz != 0) { + pidController.setIZone(iz); + } + return pidController; } } diff --git a/swervelib/parser/SwerveParser.java b/swervelib/parser/SwerveParser.java index afa5354..47ea5e2 100644 --- a/swervelib/parser/SwerveParser.java +++ b/swervelib/parser/SwerveParser.java @@ -1,5 +1,6 @@ package swervelib.parser; +import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.JsonNode; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.geometry.Pose2d; @@ -57,17 +58,21 @@ public class SwerveParser checkDirectory(directory); swerveDriveJson = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); controllerPropertiesJson = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) .readValue( new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); pidfPropertiesJson = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) .readValue( new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); physicalPropertiesJson = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) .readValue( new File(directory, "modules/physicalproperties.json"), PhysicalPropertiesJson.class); @@ -77,7 +82,9 @@ public class SwerveParser moduleConfigs.put(swerveDriveJson.modules[i], i); File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); assert moduleFile.exists(); - moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class); + moduleJsons[i] = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue(moduleFile, ModuleJson.class); } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 7c10b6b..6960783 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -6,13 +6,13 @@ 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; import swervelib.encoders.CANCoderSwerve; import swervelib.encoders.CanAndMagSwerve; -import swervelib.encoders.PWMDutyCycleEncoderSwerve; +import swervelib.encoders.DIODutyCycleEncoderSwerve; +import swervelib.encoders.SparkFlexEncoderSwerve; import swervelib.encoders.SparkMaxAnalogEncoderSwerve; import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -26,12 +26,14 @@ import swervelib.imu.CanandgyroSwerve; import swervelib.imu.NavXSwerve; import swervelib.imu.Pigeon2Swerve; import swervelib.imu.PigeonSwerve; +import swervelib.imu.PigeonViaTalonSRXSwerve; import swervelib.imu.SwerveIMU; import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve.Type; import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; +import swervelib.motors.TalonFXSSwerve; import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; import swervelib.motors.ThriftyNovaSwerve; @@ -81,6 +83,11 @@ public class DeviceJson return new SparkMaxAnalogEncoderSwerve(motor, 3.3); case "sparkmax_analog5v": return new SparkMaxAnalogEncoderSwerve(motor, 5); + case "sparkflex_integrated": + case "sparkflex_attached": + case "sparkflex_canandmag": + case "sparkflex_canandcoder": + return new SparkFlexEncoderSwerve(motor, 360); case "canandcoder_can": case "canandmag_can": return new CanAndMagSwerve(id); @@ -89,13 +96,16 @@ public class DeviceJson case "throughbore": case "am_mag": case "dutycycle": - return new PWMDutyCycleEncoderSwerve(id); + return new DIODutyCycleEncoderSwerve(id); case "thrifty": case "ma3": case "analog": return new AnalogAbsoluteEncoderSwerve(id); case "cancoder": return new CANCoderSwerve(id, canbus != null ? canbus : ""); + case "srxmag_standalone": + return new TalonSRXEncoderSwerve(new TalonSRXSwerve(id, false, DCMotor.getCIM(1)), + FeedbackDevice.PulseWidthEncodedPosition); case "talonsrx_pwm": return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition); case "talonsrx_analog": @@ -151,6 +161,8 @@ public class DeviceJson return new NavXSwerve(NavXComType.kMXP_UART); case "pigeon": return new PigeonSwerve(id); + case "pigeon_via_talonsrx": + return new PigeonViaTalonSRXSwerve(id); case "pigeon2": return new Pigeon2Swerve(id, canbus != null ? canbus : ""); default: @@ -172,10 +184,22 @@ public class DeviceJson } switch (type) { + case "talonfxs_neo": + return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNEO(1)); + case "talonfxs_neo550": + return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeo550(1)); + case "talonfxs_vortex": + return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1)); + case "talonfxs_minion": + throw new UnsupportedOperationException("Cannot create minion combination yet"); //new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1)); case "sparkmax_neo": case "neo": case "sparkmax": return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNEO(1)); + case "sparkmax_vortex": + return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1)); + case "sparkmax_minion": + throw new UnsupportedOperationException("Cannot create minion combination yet"); case "sparkmax_neo550": case "neo550": return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeo550(1)); @@ -187,6 +211,8 @@ public class DeviceJson return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNEO(1)); case "sparkflex_neo550": return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeo550(1)); + case "sparkflex_minion": + throw new UnsupportedOperationException("Cannot create minion combination yet"); case "falcon500": case "falcon": return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500(1)); @@ -230,6 +256,10 @@ public class DeviceJson return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNEO(1)); case "nova_neo550": return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeo550(1)); + case "nova_vortex": + return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1)); + case "nova_minion": + throw new UnsupportedOperationException("Cannot create minion combination");//return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getMinion(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 7cb8821..9d00c88 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -75,6 +75,14 @@ public class ModuleJson SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); + //Throw an error if module locations are improperly set + if (location.front == 0 && location.left == 0) + { + throw new RuntimeException("Improper Module Location Settings!\n" + + "Your module location is set to 0 for both 'front' and 'left' values.\n" + + "Set the distance from the center of the robot to the center of the wheel in your module JSON file!"); + } + // Set the conversion factors to null if they are both 0. if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null) { @@ -119,8 +127,8 @@ public class ModuleJson conversionFactors, absEncoder, absoluteEncoderOffset, - Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), - Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y), + Units.inchesToMeters(Math.round(location.front)), + Units.inchesToMeters(Math.round(location.left)), anglePIDF, velocityPIDF, physicalCharacteristics, diff --git a/swervelib/parser/json/PhysicalPropertiesJson.java b/swervelib/parser/json/PhysicalPropertiesJson.java index 758675d..197089d 100644 --- a/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/swervelib/parser/json/PhysicalPropertiesJson.java @@ -63,12 +63,11 @@ public class PhysicalPropertiesJson if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() && conversionFactors.isAngleEmpty()) { - new Alert("Configuration", + throw new RuntimeException( "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle + "} \nis deprecated, please use\n" + "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " + - conversionFactor.angle + "} }", - AlertType.kError).set(true); + conversionFactor.angle + "} }"); } return new SwerveModulePhysicalCharacteristics( diff --git a/swervelib/parser/json/modules/LocationJson.java b/swervelib/parser/json/modules/LocationJson.java index a0471ef..5f593c4 100644 --- a/swervelib/parser/json/modules/LocationJson.java +++ b/swervelib/parser/json/modules/LocationJson.java @@ -10,9 +10,9 @@ public class LocationJson /** * Location of the swerve module in inches from the center of the robot horizontally. */ - public double front = 0, x = 0; + public double front = 0; /** * Location of the swerve module in inches from the center of the robot vertically. */ - public double left = 0, y = 0; + public double left = 0; } diff --git a/swervelib/simulation/SwerveModuleSimulation.java b/swervelib/simulation/SwerveModuleSimulation.java index 96fe383..41bc138 100644 --- a/swervelib/simulation/SwerveModuleSimulation.java +++ b/swervelib/simulation/SwerveModuleSimulation.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation; +import swervelib.SwerveDrive; import swervelib.parser.SwerveModulePhysicalCharacteristics; /** @@ -14,12 +15,16 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics; public class SwerveModuleSimulation { - private SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null; + /** + * MapleSim module. + */ + public SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null; /** * Configure the maple sim module * - * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation + * @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, @@ -42,6 +47,31 @@ public class SwerveModuleSimulation mapleSimModule.runModuleState(desiredState); } + /** + * Runs a drive motor characterization on the sim module. This is called from + * {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double, boolean)} to run + * sysId during simulation + * + * @param desiredFacing the desired facing of the module + * @param volts the voltage to run + */ + public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts) + { + mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts); + } + + /** + * Runs a drive motor characterization on the sim module. This method is called from + * {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId + * during simulation + * + * @param volts the voltage to run + */ + public void runAngleMotorCharacterization(double volts) + { + mapleSimModule.runSteerMotorCharacterization(volts); + } + /** * Get the simulated swerve module position. * diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java index 8bd5964..a069792 100644 --- a/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/swervelib/telemetry/SwerveDriveTelemetry.java @@ -429,7 +429,7 @@ public class SwerveDriveTelemetry updateSwerveTelemetrySettings(); } measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond; - measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond; + measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vyMetersPerSecond; measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond); desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;