From 9e71f63d82d55593c2f51cbf5dfb39f72e699021 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 2 Feb 2024 23:25:27 -0500 Subject: [PATCH] Sync with GRRBase --- .vscode/settings.json | 3 +- src/main/deploy/blacklight/README.md | 3 - src/main/deploy/blacklight/layout.json | 11 - .../team340/lib/blacklight/Blacklight.java | 133 -------- .../lib/blacklight/BlacklightConfig.java | 297 ------------------ .../org/team340/lib/swerve/SwerveBase.java | 231 +++++--------- .../org/team340/lib/swerve/SwerveModule.java | 88 ++---- .../lib/swerve/config/SwerveConfig.java | 278 ++++++++-------- .../lib/swerve/config/SwerveModuleConfig.java | 12 +- .../hardware/encoders/SwerveEncoder.java | 10 + .../lib/swerve/hardware/imu/SwerveIMU.java | 8 + .../swerve/hardware/motors/SwerveMotor.java | 13 + .../motors/vendors/SwerveSparkFlex.java | 7 +- .../motors/vendors/SwerveSparkMax.java | 7 +- .../lib/swerve/util/SwerveField2d.java | 25 +- .../lib/swerve/util/SwerveOdometryThread.java | 126 ++++++++ .../lib/swerve/util/SwerveRatelimiter.java | 6 +- .../lib/swerve/util/SwerveSerializer.java | 70 +++++ src/main/java/org/team340/lib/util/Math2.java | 6 +- .../lib/util/config/rev/SparkFlexConfig.java | 14 +- .../lib/util/config/rev/SparkMaxConfig.java | 14 +- .../config/rev/SparkPIDControllerConfig.java | 112 ++++++- .../java/org/team340/robot/Constants.java | 24 +- .../org/team340/robot/subsystems/Pivot.java | 20 +- .../org/team340/robot/subsystems/Shooter.java | 6 +- 25 files changed, 625 insertions(+), 899 deletions(-) delete mode 100644 src/main/deploy/blacklight/README.md delete mode 100644 src/main/deploy/blacklight/layout.json delete mode 100644 src/main/java/org/team340/lib/blacklight/Blacklight.java delete mode 100644 src/main/java/org/team340/lib/blacklight/BlacklightConfig.java create mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java create mode 100644 src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 9b8cea9..3362302 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -32,8 +32,6 @@ "Accum", "ADIS", "Bezier", - "Blacklight", - "Blacklights", "Botpose", "Brushless", "CANandCoder", @@ -81,6 +79,7 @@ "REVRobotics", "RoboRIO", "Sendables", + "Setpoint", "Subuid", "TalonFX", "TalonSRX", diff --git a/src/main/deploy/blacklight/README.md b/src/main/deploy/blacklight/README.md deleted file mode 100644 index 9e899bd..0000000 --- a/src/main/deploy/blacklight/README.md +++ /dev/null @@ -1,3 +0,0 @@ -## deploy/blacklight - -This directory contains `Layout.json`, which describes the positions of april tags on the field. The specified layout is consumed by the Blacklight wrapper and sent via Network Tables to the Blacklight. diff --git a/src/main/deploy/blacklight/layout.json b/src/main/deploy/blacklight/layout.json deleted file mode 100644 index 6f1257a..0000000 --- a/src/main/deploy/blacklight/layout.json +++ /dev/null @@ -1,11 +0,0 @@ -[ - { - "id": 0, - "x": 0.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - } -] diff --git a/src/main/java/org/team340/lib/blacklight/Blacklight.java b/src/main/java/org/team340/lib/blacklight/Blacklight.java deleted file mode 100644 index 20d6468..0000000 --- a/src/main/java/org/team340/lib/blacklight/Blacklight.java +++ /dev/null @@ -1,133 +0,0 @@ -package org.team340.lib.blacklight; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.File; -import java.util.Scanner; - -/** - * Helper class for interfacing with Blacklights. - */ -public class Blacklight { - - /** - * The Blacklight's config. - */ - private final BlacklightConfig config; - /** - * The network table used by the Blacklight. - */ - private final NetworkTable nt; - /** - * Standard deviations for vision measurements. - */ - private final double[] std; - - private DoubleSubscriber fpsSub; - private DoubleArraySubscriber poseEstimationSub; - private DoubleArraySubscriber debugPoseEstimationSub; - - /** - * Create the Blacklight helper. - * @param config The Blacklight's config. - */ - public Blacklight(BlacklightConfig config) { - config.verify(); - this.config = config; - std = config.getStandardDeviations(); - nt = NetworkTableInstance.getDefault().getTable("/Blacklight-" + config.getName()); - } - - /** - * Publishes the Blacklight's configuration to network tables. - */ - public void publishConfig() { - String tagLayout = ""; - try { - Scanner layoutScanner = new Scanner( - new File(Filesystem.getDeployDirectory(), "blacklight/" + config.getTagLayoutFileName() + ".json") - ); - while (layoutScanner.hasNextLine()) tagLayout += layoutScanner.nextLine(); - } catch (Exception e) { - DriverStation.reportError(e.getMessage(), true); - } - - NetworkTable configTable = nt.getSubTable("config"); - configTable.getStringTopic("devicePath").publish().set(config.getDevicePath()); - configTable.getIntegerTopic("height").publish().set(config.getHeight()); - configTable.getIntegerTopic("width").publish().set(config.getWidth()); - configTable.getIntegerTopic("autoExposure").publish().set(config.getAutoExposure()); - configTable.getIntegerTopic("absoluteExposure").publish().set(config.getAbsoluteExposure()); - configTable.getIntegerTopic("gain").publish().set(config.getGain()); - configTable.getDoubleArrayTopic("cameraPosition").publish().set(config.getCameraPosition()); - configTable.getDoubleTopic("errorAmbiguity").publish().set(config.getErrorAmbiguity()); - configTable.getDoubleTopic("tagSize").publish().set(config.getTagSize()); - configTable.getStringTopic("tagFamily").publish().set(config.getTagFamily()); - configTable.getStringTopic("tagLayout").publish().set(tagLayout.length() == 0 ? "[]" : tagLayout); - configTable.getIntegerTopic("debugTag").publish().set(config.getDebugTag()); - configTable.getDoubleArrayTopic("fieldSize").publish().set(config.getFieldSize()); - configTable.getDoubleArrayTopic("fieldMargin").publish().set(config.getFieldMargin()); - } - - /** - * Start NT listeners for the Blacklight's outputs. - */ - public void startListeners() { - NetworkTable outputTable = nt.getSubTable("output"); - - if (fpsSub == null) { - fpsSub = outputTable.getDoubleTopic("fps").subscribe(0); - } - - if (poseEstimationSub == null) { - poseEstimationSub = - outputTable - .getDoubleArrayTopic("poseEstimation") - .subscribe(new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); - } - - if (debugPoseEstimationSub == null) { - debugPoseEstimationSub = - outputTable - .getDoubleArrayTopic("debugPoseEstimation") - .subscribe(new double[] {}, PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true)); - } - } - - /** - * Updates local data from the NT subscriptions. Should be ran periodically. - * @param poseEstimator The pose estimator in use to be updated. - */ - public void update(SwerveDrivePoseEstimator poseEstimator) { - TimestampedDoubleArray[] poseEstimations = poseEstimationSub.readQueue(); - for (int i = 0; i < poseEstimations.length; i++) { - if (poseEstimations[i].value.length > 0) { - Pose3d pose = new Pose3d( - poseEstimations[i].value[0], - poseEstimations[i].value[1], - poseEstimations[i].value[2], - new Rotation3d(poseEstimations[i].value[3], poseEstimations[i].value[4], poseEstimations[i].value[5]) - ); - - double distance = poseEstimations[i].value[6]; - double distanceScale = distance * distance; - - poseEstimator.addVisionMeasurement( - pose.toPose2d(), - poseEstimations[i].timestamp / 1000000.0, - VecBuilder.fill(std[0] * distanceScale, std[1] * distanceScale, std[2] * distanceScale) - ); - } - } - } -} diff --git a/src/main/java/org/team340/lib/blacklight/BlacklightConfig.java b/src/main/java/org/team340/lib/blacklight/BlacklightConfig.java deleted file mode 100644 index 17ec313..0000000 --- a/src/main/java/org/team340/lib/blacklight/BlacklightConfig.java +++ /dev/null @@ -1,297 +0,0 @@ -package org.team340.lib.blacklight; - -import java.util.MissingResourceException; - -/** - * Config builder for {@link Blacklight}. - */ -public class BlacklightConfig { - - private String name; - private String devicePath; - private int height = -1; - private int width = -1; - private int autoExposure = 0; - private int absoluteExposure = 0; - private int gain = 0; - private double[] cameraPosition; - private double errorAmbiguity = -1; - private double tagSize = -1; - private String tagFamily; - private String tagLayoutFileName; - private int debugTag = -1; - private double[] fieldSize; - private double[] fieldMargin; - private double[] standardDeviations; - - /** - * Use the Blacklight with the specified name. - * Blacklight's names are configured on device in the connection config. - * @param name The name of the Blacklight. - */ - public BlacklightConfig withName(String name) { - this.name = name; - return this; - } - - /** - * Gets the configured name of the Blacklight. - */ - public String getName() { - return name; - } - - /** - * Sets the device path of the camera. Typically this is {@code /dev/video0}. - * @param devicePath The absolute path of the camera. - */ - public BlacklightConfig setDevicePath(String devicePath) { - this.devicePath = devicePath; - return this; - } - - /** - * Gets the configured device path of the camera. - */ - public String getDevicePath() { - return devicePath; - } - - /** - * Sets the resolution of the camera in pixels. - * @param height The camera's height. - * @param width The camera's width. - */ - public BlacklightConfig setResolution(int height, int width) { - this.height = height; - this.width = width; - return this; - } - - /** - * Gets the configured resolution height of the camera. - */ - public int getHeight() { - return height; - } - - /** - * Gets the configured resolution width of the camera. - */ - public int getWidth() { - return width; - } - - /** - * Sets settings for the camera's sensor. - * @param autoExposure Auto exposure setting. A value of {@code 1} works well. - * @param absoluteExposure The absolute exposure setting. A value of {@code 10} works well. - * @param gain The gain setting. A value of {@code 25} works well. - */ - public BlacklightConfig setSensorSettings(int autoExposure, int absoluteExposure, int gain) { - this.autoExposure = autoExposure; - this.absoluteExposure = absoluteExposure; - this.gain = gain; - return this; - } - - /** - * Gets the configured auto exposure setting. - */ - public int getAutoExposure() { - return autoExposure; - } - - /** - * Gets the configured absolute exposure setting. - */ - public int getAbsoluteExposure() { - return absoluteExposure; - } - - /** - * Gets the configured gain setting. - */ - public int getGain() { - return gain; - } - - /** - * Sets the camera's position, relative to the robot's center. Units are meters and radians. - * @param x The camera's X position in meters. - * @param y The camera's Y position in meters. - * @param z The camera's Z position in meters. - * @param rx The camera's rotational X position (roll) in radians. - * @param ry The camera's rotational Y position (pitch) in radians. - * @param rz The camera's rotational Z position (yaw) in radians. - */ - public BlacklightConfig setCameraPosition(double x, double y, double z, double rx, double ry, double rz) { - cameraPosition = new double[] { x, y, z, rx, ry, rz }; - return this; - } - - /** - * Gets the configured camera position, as an array of {@code [x, y, z, rx, ry, rz]}. - */ - public double[] getCameraPosition() { - return cameraPosition; - } - - /** - * Sets the error ambiguity threshold for pose estimation when solve PnP is not in use. - * @param errorAmbiguity The error ambiguity threshold. A value of {@code 0.15} (15%) works well. - */ - public BlacklightConfig setErrorAmbiguity(double errorAmbiguity) { - this.errorAmbiguity = errorAmbiguity; - return this; - } - - /** - * Gets the configured error ambiguity threshold. - */ - public double getErrorAmbiguity() { - return errorAmbiguity; - } - - /** - * Sets properties for the april tags. - * The tag layout file should follow the following format: - * 
-     *[
-     *  {
-     *    "id": integer
-     *    "x": float
-     *    "y": float
-     *    "z": float
-     *    "rx": float
-     *    "ry": float
-     *    "rz": float
-     *  },
-     *  ...
-     *]
-     *
- * @param tagSize The size (length) of the tags in meters. - * @param tagFamily The tag family ({@code 16h5}, {@code 36h11}, etc) in use. This is case sensitive. - * @param tagLayoutFileName The name of the JSON file in {@code deploy/blacklight} that specifies the layout of tags on the field (do not include {@code .json}). - */ - public BlacklightConfig setTagProperties(double tagSize, String tagFamily, String tagLayoutFileName) { - this.tagSize = tagSize; - this.tagFamily = tagFamily; - this.tagLayoutFileName = tagLayoutFileName; - return this; - } - - /** - * Gets the configured tag size in meters. - */ - public double getTagSize() { - return tagSize; - } - - /** - * Gets the configured tag family. - */ - public String getTagFamily() { - return tagFamily; - } - - /** - * Gets the configured tag layout file name. - */ - public String getTagLayoutFileName() { - return tagLayoutFileName; - } - - /** - * Sets a tag ID to be used for debugging. - * @param debugTag The ID of the tag to use for debugging. {@code -1} disables the debug tag. - */ - public BlacklightConfig setDebugTag(int debugTag) { - this.debugTag = debugTag; - return this; - } - - /** - * Gets the configured debug tag. - */ - public int getDebugTag() { - return debugTag; - } - - /** - * Sets the size of the field. - * @param fieldLength The field's length in meters. Typically {@code 16.5417}. - * @param fieldWidth The field's width in meters. Typically {@code 8.0136}. - * @param z If the field has vertical elements the robot can drive onto, this should be set to the highest point in meters. Otherwise, use {@code 0.0}. - */ - public BlacklightConfig setFieldSize(double fieldLength, double fieldWidth, double z) { - fieldSize = new double[] { fieldLength, fieldWidth, z }; - return this; - } - - /** - * Gets the configured field size, as an array of {@code [fieldLength, fieldWidth, z]}. - */ - public double[] getFieldSize() { - return fieldSize; - } - - /** - * Sets margins on the field's outer bounds to still accept vision measurements from. - * @param lengthMargin The margin for the field's length. - * @param widthMargin The margin for the field's width. - * @param zMargin The margin for the field's Z. - */ - public BlacklightConfig setFieldMargin(double lengthMargin, double widthMargin, double zMargin) { - fieldMargin = new double[] { lengthMargin, widthMargin, zMargin }; - return this; - } - - /** - * Gets the configured field margin, as an array of {@code [lengthMargin, widthMargin, zMargin]}. - */ - public double[] getFieldMargin() { - return fieldMargin; - } - - /** - * Sets standard deviations for vision measurements. - * A good starting configuration is all axis with a magnitude of {@code 0.4}. - * @param x The X axis standard deviation in meters. - * @param y The Y axis standard deviation in meters. - * @param rot The rotational standard deviation in radians. - */ - public BlacklightConfig setStandardDeviations(double x, double y, double rot) { - this.standardDeviations = new double[] { x, y, rot }; - return this; - } - - /** - * Gets the configured standard deviations for vision measurements, as an array of {@code [x, y, rot]}. - */ - public double[] getStandardDeviations() { - return standardDeviations; - } - - /** - * Verifies the config. - */ - public void verify() { - if (name == null) throwMissing("Name"); - if (devicePath == null) throwMissing("Camera Device Path"); - if (height == -1) throwMissing("Height"); - if (width == -1) throwMissing("Width"); - if (cameraPosition == null) throwMissing("Camera Position"); - if (errorAmbiguity == -1) throwMissing("Error Ambiguity"); - if (tagSize == -1) throwMissing("Tag Size"); - if (tagFamily == null) throwMissing("Tag Family"); - if (tagLayoutFileName == null) throwMissing("Tag Layout File Name"); - if (fieldSize == null) throwMissing("Field Size"); - if (fieldMargin == null) throwMissing("Field Margin"); - if (standardDeviations == null) throwMissing("Standard Deviations"); - } - - private void throwMissing(String key) { - throw new MissingResourceException("Missing value: " + key, this.getClass().getSimpleName(), key); - } -} diff --git a/src/main/java/org/team340/lib/swerve/SwerveBase.java b/src/main/java/org/team340/lib/swerve/SwerveBase.java index eeed6ae..e94cc87 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveBase.java +++ b/src/main/java/org/team340/lib/swerve/SwerveBase.java @@ -9,7 +9,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; -import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; @@ -29,22 +28,14 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.ADIS16470_IMU; -import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import java.util.ArrayDeque; -import java.util.ArrayList; -import java.util.Deque; -import java.util.Iterator; -import java.util.List; -import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; import org.team340.lib.GRRDashboard; import org.team340.lib.GRRSubsystem; -import org.team340.lib.blacklight.Blacklight; -import org.team340.lib.blacklight.BlacklightConfig; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; @@ -60,7 +51,9 @@ import org.team340.lib.swerve.hardware.motors.vendors.SwerveTalonFX; import org.team340.lib.swerve.util.SwerveConversions; import org.team340.lib.swerve.util.SwerveField2d; +import org.team340.lib.swerve.util.SwerveOdometryThread; import org.team340.lib.swerve.util.SwerveRatelimiter; +import org.team340.lib.swerve.util.SwerveSerializer; import org.team340.lib.util.Math2; import org.team340.lib.util.SendableFactory; import org.team340.lib.util.StringUtil; @@ -81,65 +74,33 @@ * Supported encoders: * * * Supported IMUs: * */ public abstract class SwerveBase extends GRRSubsystem { - /** - * Supported IMUs. - */ - public static enum SwerveIMUType { - ADIS16470, - PIGEON2, - } - - /** - * Supported motors. - */ - public static enum SwerveMotorType { - SPARK_MAX_BRUSHED, - SPARK_MAX_BRUSHLESS, - SPARK_FLEX_BRUSHED, - SPARK_FLEX_BRUSHLESS, - TALONFX, - } - - /** - * Supported encoders. - */ - public static enum SwerveEncoderType { - CANCODER, - SPARK_ENCODER, - } - protected final SwerveConfig config; - protected final SwerveIMU imu; - protected final SwerveModule[] modules; - protected final Translation2d[] moduleTranslations; protected final SwerveConversions conversions; + protected final SwerveIMU imu; protected final SwerveDriveKinematics kinematics; - protected final SwerveRatelimiter ratelimiter; - protected final SwerveField2d field; + protected final SwerveModule[] modules; protected final SwerveDrivePoseEstimator poseEstimator; - protected final List blacklights = new ArrayList<>(); + protected final SysIdRoutine sysIdRoutine; - protected final Notifier odometryThread; - protected final ReentrantLock odometryMutex = new ReentrantLock(); - protected final Deque timestampQueue = new ArrayDeque<>(); - protected final Deque imuYawQueue = new ArrayDeque<>(); + private final Translation2d[] moduleTranslations; + private final SwerveOdometryThread odometryThread; + private final SwerveRatelimiter ratelimiter; + private final SwerveField2d field; - protected final MutableMeasure sysIdAppliedVoltage = mutable(Volts.of(0)); - protected final MutableMeasure sysIdDistance = mutable(Meters.of(0)); - protected final MutableMeasure> sysIdVelocity = mutable(MetersPerSecond.of(0)); - - protected final SysIdRoutine sysIdRoutine; + private final MutableMeasure sysIdAppliedVoltage = mutable(Volts.of(0)); + private final MutableMeasure sysIdDistance = mutable(Meters.of(0)); + private final MutableMeasure> sysIdVelocity = mutable(MetersPerSecond.of(0)); /** * Create the GRRSwerve subsystem. @@ -163,10 +124,11 @@ public SwerveBase(String label, SwerveConfig config) { conversions = new SwerveConversions(config); kinematics = new SwerveDriveKinematics(moduleTranslations); + odometryThread = new SwerveOdometryThread(modules, imu::getYaw, config); ratelimiter = new SwerveRatelimiter(config, kinematics, getModuleStates()); - field = new SwerveField2d(config, moduleTranslations); + field = new SwerveField2d(config); - double[] std = config.getStandardDeviations(); + double[] std = config.getOdometryStd(); poseEstimator = new SwerveDrivePoseEstimator( kinematics, @@ -177,23 +139,10 @@ public SwerveBase(String label, SwerveConfig config) { VecBuilder.fill(0.0, 0.0, 0.0) ); - for (BlacklightConfig blacklightConfig : config.getBlacklights()) { - Blacklight blacklight = new Blacklight(blacklightConfig); - blacklight.publishConfig(); - blacklight.startListeners(); - blacklights.add(blacklight); - } - - odometryThread = new Notifier(this::sampleOdometry); - odometryThread.setName("Swerve Odometry"); - odometryThread.startPeriodic(config.getOdometryPeriod()); - sysIdRoutine = new SysIdRoutine( - // Default config, rampRate is 1V/sec, stepVoltage is 7V, and timeout is 10secs config.getSysIdConfig(), new Mechanism( - // Defines how drive command should be sent to motors (Measure volts) -> { driveVoltage(volts.in(Volts), Math2.ROTATION2D_0); }, @@ -214,6 +163,7 @@ public SwerveBase(String label, SwerveConfig config) { ); imu.setZero(Math2.ROTATION2D_0); + odometryThread.start(); System.out.println( "\nGRRSwerve Conversions:" + @@ -233,36 +183,28 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("velocityX", () -> Math2.toFixed(getVelocity(true).vxMetersPerSecond), null); builder.addDoubleProperty("velocityY", () -> Math2.toFixed(getVelocity(true).vyMetersPerSecond), null); builder.addDoubleProperty("velocityRot", () -> Math2.toFixed(getVelocity(true).omegaRadiansPerSecond), null); - builder.addDoubleProperty( - "velocityNorm", - () -> { - ChassisSpeeds velocity = getVelocity(true); - return Math2.toFixed(Math.hypot(velocity.vxMetersPerSecond, velocity.vyMetersPerSecond)); - }, - null - ); + builder.addDoubleProperty("velocityNorm", () -> SwerveSerializer.chassisSpeedsNorm(getVelocity(true), true), null); builder.addDoubleProperty("odometryX", () -> Math2.toFixed(getPosition().getX()), null); builder.addDoubleProperty("odometryY", () -> Math2.toFixed(getPosition().getY()), null); builder.addDoubleProperty("odometryRot", () -> Math2.toFixed(getPosition().getRotation().getRadians()), null); + builder.addDoubleArrayProperty( + "desiredModuleStates", + () -> SwerveSerializer.moduleStatesDoubleArray(getDesiredModuleStates(), true), + null + ); + builder.addDoubleArrayProperty("moduleStates", () -> SwerveSerializer.moduleStatesDoubleArray(getModuleStates(), true), null); + for (SwerveModule module : modules) { GRRDashboard.addSubsystemSendable( "Modules/" + StringUtil.toPascalCase(module.getLabel()), this, SendableFactory.create(moduleBuilder -> { moduleBuilder.publishConstString(".label", module.getLabel()); - moduleBuilder.addDoubleProperty( - "velocity", - () -> Math2.toFixed(module.getVelocity()), - v -> module.setDesiredState(new SwerveModuleState(v, Rotation2d.fromRadians(module.getHeading()))) - ); + moduleBuilder.addDoubleProperty("velocity", () -> Math2.toFixed(module.getVelocity()), null); moduleBuilder.addDoubleProperty("distance", () -> Math2.toFixed(module.getDistance()), null); - moduleBuilder.addDoubleProperty( - "angle", - () -> Math2.toFixed(module.getHeading()), - v -> module.setDesiredState(new SwerveModuleState(0.0, Rotation2d.fromRadians(v))) - ); + moduleBuilder.addDoubleProperty("angle", () -> Math2.toFixed(module.getHeading()), null); }) ); } @@ -270,24 +212,6 @@ public void initSendable(SendableBuilder builder) { GRRDashboard.addSubsystemSendable("Field", this, field); } - /** - * Gets the states of all swerve modules. - */ - protected SwerveModuleState[] getModuleStates() { - SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; - for (int i = 0; i < moduleStates.length; i++) moduleStates[i] = modules[i].getModuleState(); - return moduleStates; - } - - /** - * Gets the positions of all swerve modules. - */ - protected SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; - for (int i = 0; i < modulePositions.length; i++) modulePositions[i] = modules[i].getModulePosition(); - return modulePositions; - } - /** * Gets the robot's velocity. * @param fieldRelative If the returned velocity should be field relative. @@ -308,19 +232,30 @@ protected Pose2d getPosition() { } /** - * Samples a timestamp, IMU yaw, and module positions to odometry sample queues. + * Gets the desired states of all swerve modules. */ - protected void sampleOdometry() { - try { - odometryMutex.lock(); - timestampQueue.add(MathSharedStore.getTimestamp()); - imuYawQueue.add(imu.getYaw()); - for (SwerveModule module : modules) { - module.recordSample(); - } - } finally { - odometryMutex.unlock(); - } + protected SwerveModuleState[] getDesiredModuleStates() { + SwerveModuleState[] desiredModuleStates = new SwerveModuleState[modules.length]; + for (int i = 0; i < desiredModuleStates.length; i++) desiredModuleStates[i] = modules[i].getDesiredState(); + return desiredModuleStates; + } + + /** + * Gets the states of all swerve modules. + */ + protected SwerveModuleState[] getModuleStates() { + SwerveModuleState[] moduleStates = new SwerveModuleState[modules.length]; + for (int i = 0; i < moduleStates.length; i++) moduleStates[i] = modules[i].getModuleState(); + return moduleStates; + } + + /** + * Gets the positions of all swerve modules. + */ + protected SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; + for (int i = 0; i < modulePositions.length; i++) modulePositions[i] = modules[i].getModulePosition(); + return modulePositions; } /** @@ -337,36 +272,18 @@ protected void resetOdometry(Pose2d newPose) { * Should be ran periodically. */ protected void updateOdometry() { - try { - odometryMutex.lock(); - for (Blacklight blacklight : blacklights) blacklight.update(poseEstimator); - Iterator timestampIterator = timestampQueue.iterator(); - while (timestampIterator.hasNext()) { - SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; - boolean missingModule = false; - for (int i = 0; i < modulePositions.length; i++) { - double distance = modules[i].pollDistanceSample(); - double heading = modules[i].pollHeadingSample(); - if (distance == Double.MIN_VALUE || heading == Double.MIN_VALUE) { - missingModule = true; - continue; - } - modulePositions[i] = new SwerveModulePosition(distance, new Rotation2d(heading)); - } - double timestamp = timestampIterator.next(); - if (missingModule) continue; - try { - Rotation2d yaw = imuYawQueue.pop(); - poseEstimator.updateWithTime(timestamp, yaw, modulePositions); - } catch (Exception e) { - continue; - } - } - timestampQueue.clear(); - field.update(getPosition(), getModulePositions()); - } finally { - odometryMutex.unlock(); - } + updateOdometry(poseEstimator -> {}); + } + + /** + * Updates odometry. + * Should be ran periodically. + * @param poseEstimatorConsumer A consumer that accepts the pose estimator. Should be used for applying field-relative poses from vision data. Note that it is expected that standard deviations are specified when using {@link SwerveDrivePoseEstimator#addVisionMeasurement}, as the initial standard deviations are set to {@code 0.0}. + */ + protected void updateOdometry(Consumer poseEstimatorConsumer) { + odometryThread.update(poseEstimator); + poseEstimatorConsumer.accept(poseEstimator); + field.update(getPosition()); } /** @@ -397,7 +314,7 @@ protected void stop() { * @param fieldRelative If the robot should drive field relative. */ protected void drive(double x, double y, double rot, boolean fieldRelative) { - driveVelocity(x * config.getMaxV(), y * config.getMaxV(), rot * config.getMaxRv(), fieldRelative); + driveVelocity(x * config.getVelocity(), y * config.getVelocity(), rot * config.getRotationalVelocity(), fieldRelative); } /** @@ -423,7 +340,7 @@ protected void driveVelocity(double xV, double yV, double rotV, boolean fieldRel * @param controller A profiled PID controller to use for translating to and maintaining the angle to the desired point. */ protected void driveAroundPoint(double x, double y, Translation2d point, ProfiledPIDController controller) { - driveAroundPointVelocity(x * config.getMaxV(), y * config.getMaxV(), point, controller); + driveAroundPointVelocity(x * config.getVelocity(), y * config.getVelocity(), point, controller); } /** @@ -447,7 +364,7 @@ protected void driveAroundPointVelocity(double xV, double yV, Translation2d poin * @param controller A profiled PID controller to use for translating to and maintaining the angle. */ protected void driveAngle(double x, double y, double angle, ProfiledPIDController controller) { - driveAngleVelocity(x * config.getMaxV(), y * config.getMaxV(), angle, controller); + driveAngleVelocity(x * config.getVelocity(), y * config.getVelocity(), angle, controller); } /** @@ -529,7 +446,7 @@ protected void driveSpeeds(ChassisSpeeds chassisSpeeds, boolean discretize, bool driveStates(ratelimiter.calculate(chassisSpeeds).moduleStates()); } else { SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(states, config.getMaxV()); + SwerveDriveKinematics.desaturateWheelSpeeds(states, config.getVelocity()); ratelimiter.setLastState(chassisSpeeds, states); driveStates(states); } @@ -615,14 +532,14 @@ private SwerveModule createModule(SwerveModuleConfig moduleConfig) { break; case SPARK_ENCODER: if ( - config.getMoveMotorType().equals(SwerveMotorType.SPARK_MAX_BRUSHED) || - config.getMoveMotorType().equals(SwerveMotorType.SPARK_MAX_BRUSHLESS) + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHED) || + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) ) { turnSparkMax = createSparkMax( moduleConfig.getLabel() + "Turn Motor", moduleConfig.getTurnMotorDeviceId(), - config.getMoveMotorType().equals(SwerveMotorType.SPARK_MAX_BRUSHLESS) + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed ); @@ -640,7 +557,7 @@ private SwerveModule createModule(SwerveModuleConfig moduleConfig) { createSparkFlex( moduleConfig.getLabel() + "Turn Motor", moduleConfig.getTurnMotorDeviceId(), - config.getMoveMotorType().equals(SwerveMotorType.SPARK_FLEX_BRUSHLESS) + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed ); @@ -690,7 +607,7 @@ private SwerveMotor createMotor(boolean isMoveMotor, SwerveEncoder encoder, Swer createSparkMax( label, deviceId, - config.getMoveMotorType().equals(SwerveMotorType.SPARK_MAX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed ), encoder, config, @@ -703,7 +620,7 @@ private SwerveMotor createMotor(boolean isMoveMotor, SwerveEncoder encoder, Swer createSparkFlex( label, deviceId, - config.getMoveMotorType().equals(SwerveMotorType.SPARK_FLEX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed + config.getMoveMotorType().equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) ? MotorType.kBrushless : MotorType.kBrushed ), encoder, config, diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index 31056b5..249b5fd 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -6,8 +6,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; -import java.util.ArrayDeque; -import java.util.Deque; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; @@ -17,7 +15,7 @@ /** * A swerve module for {@link SwerveBase} */ -class SwerveModule { +public class SwerveModule { private final SwerveConfig config; private final SwerveModuleConfig moduleConfig; @@ -27,9 +25,7 @@ class SwerveModule { private final SimpleMotorFeedforward moveFFController; private final Timer controlTimer = new Timer(); - private final Deque distanceQueue = new ArrayDeque<>(); - private final Deque headingQueue = new ArrayDeque<>(); - + private SwerveModuleState desiredState = new SwerveModuleState(); private double lastMoveSpeed = 0.0; private double simDistance = 0.0; private double simHeading = 0.0; @@ -43,7 +39,7 @@ class SwerveModule { * @param config The general swerve config. * @param moduleConfig The module's config. */ - public SwerveModule( + SwerveModule( SwerveMotor moveMotor, SwerveMotor turnMotor, SwerveEncoder encoder, @@ -66,6 +62,17 @@ public String getLabel() { return moduleConfig.getLabel(); } + /** + * Gets current duty cycle of move motor. + */ + public double getMoveDutyCycle() { + if (RobotBase.isSimulation()) { + return (simVelocity / config.getVelocity()) * 12.0; + } else { + return moveMotor.getDutyCycle(); + } + } + /** * Gets the velocity of the swerve module in meters/second. */ @@ -88,17 +95,6 @@ public double getDistance() { } } - /** - * Gets current duty cycle of move motor. - */ - public double getMoveDutyCycle() { - if (RobotBase.isSimulation()) { - return (simVelocity / config.getMaxV()) * 12.0; - } else { - return moveMotor.getDutyCycle(); - } - } - /** * Gets the heading of the swerve module in radians. */ @@ -110,6 +106,13 @@ public double getHeading() { } } + /** + * Gets the desired state of the swerve module. + */ + public SwerveModuleState getDesiredState() { + return desiredState; + } + /** * Gets the current state of the swerve module. */ @@ -125,9 +128,8 @@ public SwerveModulePosition getModulePosition() { } /** - * Sets a desired state of the swerve module. + * Sets the desired state of the swerve module. * @param state The new state. - * @return If the module was flipped. */ public void setDesiredState(SwerveModuleState state) { double moveSpeed = state.speedMetersPerSecond; @@ -155,6 +157,7 @@ public void setDesiredState(SwerveModuleState state) { simVelocity = moveSpeed; } + desiredState = state; lastMoveSpeed = moveSpeed; controlTimer.restart(); } @@ -170,48 +173,21 @@ public void setVoltage(double voltage, Rotation2d heading) { moveMotor.setVoltage(voltage); turnMotor.setReference(turnTarget, 0.0); + double expectedVelocity = + moveFFController.maxAchievableVelocity( + config.getOptimalVoltage(), + controlTimer.get() == 0 ? 0.0 : (simVelocity - lastMoveSpeed) / controlTimer.get() + ) * + (voltage / config.getOptimalVoltage()); + if (RobotBase.isSimulation()) { simDistance += simVelocity * controlTimer.get(); simHeading = heading.getRadians(); - simVelocity = - moveFFController.maxAchievableVelocity( - config.getOptimalVoltage(), - controlTimer.get() == 0 ? 0.0 : (simVelocity - lastMoveSpeed) / controlTimer.get() - ) * - (voltage / config.getOptimalVoltage()); + simVelocity = expectedVelocity; } + desiredState = new SwerveModuleState(expectedVelocity, heading); lastMoveSpeed = RobotBase.isSimulation() ? simVelocity : getVelocity(); controlTimer.restart(); } - - /** - * Adds the module's current distance and heading to their respective queues for odometry. - */ - public void recordSample() { - distanceQueue.add(getDistance()); - headingQueue.add(getHeading()); - } - - /** - * Polls first distance value in the odometry sample queue. - */ - public double pollDistanceSample() { - try { - return distanceQueue.pop(); - } catch (Exception e) { - return Double.MIN_VALUE; - } - } - - /** - * Polls first heading value in the odometry sample queue. - */ - public double pollHeadingSample() { - try { - return headingQueue.pop(); - } catch (Exception e) { - return Double.MIN_VALUE; - } - } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java index 886e179..29aec8e 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java @@ -7,10 +7,9 @@ import java.util.List; import java.util.MissingResourceException; import java.util.function.Consumer; -import org.team340.lib.blacklight.BlacklightConfig; -import org.team340.lib.swerve.SwerveBase.SwerveEncoderType; -import org.team340.lib.swerve.SwerveBase.SwerveIMUType; -import org.team340.lib.swerve.SwerveBase.SwerveMotorType; +import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; +import org.team340.lib.swerve.hardware.imu.SwerveIMU; +import org.team340.lib.swerve.hardware.motors.SwerveMotor; import org.team340.lib.util.config.FeedForwardConfig; import org.team340.lib.util.config.PIDConfig; @@ -19,7 +18,7 @@ */ public class SwerveConfig { - private SwerveIMUType imuType; + private SwerveIMU.Type imuType; private Object[] imuArgs; private double period = -1.0; private PIDConfig movePID; @@ -27,26 +26,25 @@ public class SwerveConfig { private PIDConfig turnPID; private double moveRampRate = -1.0; private double turnRampRate = -1.0; + private SwerveMotor.Type moveMotorType; + private SwerveMotor.Type turnMotorType; + private double velocity = -1.0; + private double rotationalVelocity = -1.0; + private double acceleration = -1.0; + private double moduleRotationalVelocity = -1.0; private double optimalVoltage = -1.0; private double moveCurrentLimit = -1.0; private double turnCurrentLimit = -1.0; private double moveGearRatio = -1.0; private double turnGearRatio = -1.0; private double wheelDiameterInches = -1.0; - private double maxV = -1.0; - private double maxRv = -1.0; - private double maxA = -1.0; - private double maxModuleRv = -1.0; - private SwerveMotorType moveMotorType; - private SwerveMotorType turnMotorType; private double discretizationLookahead = -1.0; private double odometryPeriod = -1.0; - private double[] standardDeviations; + private double[] odometryStd; private Config sysIdConfig = null; private double fieldLength = -1.0; private double fieldWidth = -1.0; private List modules = new ArrayList<>(); - private List blacklights = new ArrayList<>(); /** * Use an ADIS16470 IMU. @@ -63,7 +61,7 @@ public SwerveConfig useADIS16470( SPI.Port port, ADIS16470_IMU.CalibrationTime calibrationTime ) { - imuType = SwerveIMUType.ADIS16470; + imuType = SwerveIMU.Type.ADIS16470; imuArgs = new Object[] { yawAxis, pitchAxis, rollAxis, port, calibrationTime }; return this; } @@ -82,7 +80,7 @@ public SwerveConfig usePigeon2(int deviceId) { * @param canBus The name of the CAN bus being used. */ public SwerveConfig usePigeon2(int deviceId, String canBus) { - imuType = SwerveIMUType.PIGEON2; + imuType = SwerveIMU.Type.PIGEON2; imuArgs = new Object[] { deviceId, canBus }; return this; } @@ -90,7 +88,7 @@ public SwerveConfig usePigeon2(int deviceId, String canBus) { /** * Gets the selected IMU's type. */ - public SwerveIMUType getImuType() { + public SwerveIMU.Type getImuType() { return imuType; } @@ -180,6 +178,7 @@ public PIDConfig getTurnPID() { /** * Sets the motor ramp rate. + * Increase this to reduce the load on the motors. * @param moveRampRate Time in seconds to go from {@code 0.0} to full throttle on the move motors. * @param turnRampRate Time in seconds to go from {@code 0.0} to full throttle on the turn motors. */ @@ -204,156 +203,172 @@ public double getTurnRampRate() { } /** - * Sets power properties. - * @param optimalVoltage Optimal voltage to run at. Should be {@code 12}. - * @param moveCurrentLimit A current limit in amps for move motors. This is typically {@code 40}. - * @param turnCurrentLimit A current limit in amps for turn motors. This is typically {@code 30}. + * Sets the motor types used. + * @param moveMotorType The move motor type. + * @param turnMotorType The turn motor type. */ - public SwerveConfig setPowerProperties(double optimalVoltage, double moveCurrentLimit, double turnCurrentLimit) { - this.optimalVoltage = optimalVoltage; - this.moveCurrentLimit = moveCurrentLimit; - this.turnCurrentLimit = turnCurrentLimit; + public SwerveConfig setMotorTypes(SwerveMotor.Type moveMotorType, SwerveMotor.Type turnMotorType) { + this.moveMotorType = moveMotorType; + this.turnMotorType = turnMotorType; return this; } /** - * Gets the configured optimal voltage. - */ - public double getOptimalVoltage() { - return optimalVoltage; - } - - /** - * Gets the configured current limit for move motors in amps. + * Gets the move motor type. */ - public double getMoveCurrentLimit() { - return moveCurrentLimit; + public SwerveMotor.Type getMoveMotorType() { + return moveMotorType; } /** - * Gets the configured current limit for turn motors in amps. + * Gets the turn motor type. */ - public double getTurnCurrentLimit() { - return turnCurrentLimit; + public SwerveMotor.Type getTurnMotorType() { + return turnMotorType; } /** - * Sets swerve gearing properties. - * @param moveGearRatio The move gear ratio (inverse of the gearing reduction). - * @param turnGearRatio The turn gear ratio (inverse of the gearing reduction). - * @param wheelDiameterInches The wheel diameter in inches. + * Sets max speed constraints. + * These are used for constraining the requested velocity commanded to swerve modules, as well as scaling when driving by a percent of max speed. + * + *

+ * You may find more predictable behavior by setting these values lower than the actual maximum capabilities of your robot. + * It is recommended that these values are tested for using an actual robot. An easy way to do so is to set these values to an impossibly high value, then examine the outputs in network tables. + * Initial theoretical values can be estimated using the following formulas: + * + *

+ * Max Robot Velocity: {@code ( * 0.80 / 60) / ( / ( * PI))} + * + *

+ * Max Robot Rotational Velocity: {@code ( / (^2 + ^2)^0.5) * 2} + * + * @param velocity The maximum velocity the robot is capable of in meters/second. + * @param rotationalVelocity The maximum rotational velocity the robot is capable of in radians/second. */ - public SwerveConfig setMechanicalProperties(double moveGearRatio, double turnGearRatio, double wheelDiameterInches) { - this.moveGearRatio = moveGearRatio; - this.turnGearRatio = turnGearRatio; - this.wheelDiameterInches = wheelDiameterInches; + public SwerveConfig setMaxSpeeds(double velocity, double rotationalVelocity) { + this.velocity = velocity; + this.rotationalVelocity = rotationalVelocity; return this; } /** - * Gets the configured gear ratio for the move motors. - */ - public double getMoveGearRatio() { - return moveGearRatio; - } - - /** - * Gets the configured gear ratio for the turn motors. + * Gets the configured maximum robot velocity in meters/second. */ - public double getTurnGearRatio() { - return turnGearRatio; + public double getVelocity() { + return velocity; } /** - * Gets the configured wheel diameter. + * Gets the configured maximum robot rotational velocity in radians/second. */ - public double getWheelDiameterInches() { - return wheelDiameterInches; + public double getRotationalVelocity() { + return rotationalVelocity; } /** - * Sets speed constraints. + * Sets constraints for the ratelimiter. + * + *

* You may find more predictable behavior by setting these values lower than the actual maximum capabilities of your robot. * It is recommended that these values are tested for using an actual robot. An easy way to do so is to set these values to an impossibly high value, then examine the outputs in network tables. * Initial theoretical values can be estimated using the following formulas: * *

- * Max Robot Velocity: {@code ( * 0.80 / 60) / ( / ( * PI))} - * - *

* Max Robot Acceleration: {@code * 2} (VERY much an estimate, typical ballpark acceleration for robots weighing ~120 pounds) * Can also be pulled from Choreo. * *

- * Max Robot Rotational Velocity: {@code ( / (^2 + ^2)^0.5) * 2} - * - *

* Max Module Rotational Velocity: {@code ( / 60) / ( / (PI * 2)) * 0.7} * - * @param maxV The maximum velocity the robot is capable of in meters/second. - * @param maxRv The maximum rotational velocity the robot is capable of in radians/second. - * @param maxA The maximum acceleration the robot is capable of in meters/second/second. - * @param maxModuleRv The maximum rotational acceleration of a single swerve module in radians/second/second. - */ - public SwerveConfig setSpeedConstraints(double maxV, double maxRv, double maxA, double maxModuleRv) { - this.maxV = maxV; - this.maxRv = maxRv; - this.maxA = maxA; - this.maxModuleRv = maxModuleRv; + * @param acceleration The maximum acceleration the robot is capable of in meters/second/second. + * @param moduleRotationalVelocity The maximum module rotational velocity the robot is capable of in radians/second. + */ + public SwerveConfig setRatelimits(double acceleration, double moduleRotationalVelocity) { + this.acceleration = acceleration; + this.moduleRotationalVelocity = moduleRotationalVelocity; return this; } /** - * Gets the configured maximum robot velocity in meters/second. + * Gets the configured maximum robot acceleration in meters/second/second. */ - public double getMaxV() { - return maxV; + public double getAcceleration() { + return acceleration; } /** - * Gets the configured maximum robot rotational velocity in radians/second. + * Gets the configured maximum module rotational velocity in radians/second. */ - public double getMaxRv() { - return maxRv; + public double getModuleRotationalVelocity() { + return moduleRotationalVelocity; } /** - * Gets the configured maximum robot acceleration in meters/second/second. + * Sets power properties. + * @param optimalVoltage Optimal voltage to run at. Should be {@code 12}. + * @param moveCurrentLimit A current limit in amps for move motors. This is typically {@code 40}. + * @param turnCurrentLimit A current limit in amps for turn motors. This is typically {@code 30}. */ - public double getMaxA() { - return maxA; + public SwerveConfig setPowerProperties(double optimalVoltage, double moveCurrentLimit, double turnCurrentLimit) { + this.optimalVoltage = optimalVoltage; + this.moveCurrentLimit = moveCurrentLimit; + this.turnCurrentLimit = turnCurrentLimit; + return this; } /** - * Gets the configured maximum module rotational velocity in radians/second. + * Gets the configured optimal voltage. */ - public double getMaxModuleRv() { - return maxModuleRv; + public double getOptimalVoltage() { + return optimalVoltage; } /** - * Sets the motor types used. - * @param moveMotorType The move motor type. - * @param turnMotorType The turn motor type. + * Gets the configured current limit for move motors in amps. */ - public SwerveConfig setMotorTypes(SwerveMotorType moveMotorType, SwerveMotorType turnMotorType) { - this.moveMotorType = moveMotorType; - this.turnMotorType = turnMotorType; + public double getMoveCurrentLimit() { + return moveCurrentLimit; + } + + /** + * Gets the configured current limit for turn motors in amps. + */ + public double getTurnCurrentLimit() { + return turnCurrentLimit; + } + + /** + * Sets swerve gearing properties. + * @param moveGearRatio The move gear ratio (inverse of the gearing reduction). + * @param turnGearRatio The turn gear ratio (inverse of the gearing reduction). + * @param wheelDiameterInches The wheel diameter in inches. + */ + public SwerveConfig setMechanicalProperties(double moveGearRatio, double turnGearRatio, double wheelDiameterInches) { + this.moveGearRatio = moveGearRatio; + this.turnGearRatio = turnGearRatio; + this.wheelDiameterInches = wheelDiameterInches; return this; } /** - * Gets the move motor type. + * Gets the configured gear ratio for the move motors. */ - public SwerveMotorType getMoveMotorType() { - return moveMotorType; + public double getMoveGearRatio() { + return moveGearRatio; } /** - * Gets the turn motor type. + * Gets the configured gear ratio for the turn motors. */ - public SwerveMotorType getTurnMotorType() { - return turnMotorType; + public double getTurnGearRatio() { + return turnGearRatio; + } + + /** + * Gets the configured wheel diameter. + */ + public double getWheelDiameterInches() { + return wheelDiameterInches; } /** @@ -399,16 +414,16 @@ public double getOdometryPeriod() { * @param y The Y axis standard deviation in meters. * @param rot The rotational standard deviation in radians. */ - public SwerveConfig setStandardDeviations(double x, double y, double rot) { - this.standardDeviations = new double[] { x, y, rot }; + public SwerveConfig setOdometryStd(double x, double y, double rot) { + this.odometryStd = new double[] { x, y, rot }; return this; } /** * Gets the configured standard deviations for odometry, as an array of {@code [x, y, rot]}. */ - public double[] getStandardDeviations() { - return standardDeviations; + public double[] getOdometryStd() { + return odometryStd; } /** @@ -480,35 +495,6 @@ public List getModules() { return modules; } - /** - * Adds a Blacklight to the config. - * See {@link BlacklightConfig}. - * @param blacklightConfig The config of the Blacklight to add. - */ - public SwerveConfig addBlacklight(BlacklightConfig blacklightConfig) { - blacklights.add(blacklightConfig); - return this; - } - - /** - * Adds a Blacklight to the config. - * See {@link BlacklightConfig}. - * @param blacklightConfigConsumer A consumer for the Blacklight config. - */ - public SwerveConfig addBlacklight(Consumer blacklightConfigConsumer) { - BlacklightConfig blacklightConfig = new BlacklightConfig(); - blacklightConfigConsumer.accept(blacklightConfig); - blacklights.add(blacklightConfig); - return this; - } - - /** - * Gets configured Blacklights. - */ - public List getBlacklights() { - return blacklights; - } - /** * Verifies the config as well as the config's modules. * Throws an error if an issue is found. @@ -522,21 +508,21 @@ public void verify() { if (turnPID == null) throwMissing("Turn PID"); if (moveRampRate == -1) throwMissing("MoveRamp Rate"); if (turnRampRate == -1) throwMissing("Turn Ramp Rate"); + if (moveMotorType == null) throwMissing("Move Motor Type"); + if (turnMotorType == null) throwMissing("Turn Motor Type"); + if (velocity == -1) throwMissing("Velocity"); + if (rotationalVelocity == -1) throwMissing("Rotational Velocity"); + if (acceleration == -1) throwMissing("Acceleration"); + if (moduleRotationalVelocity == -1) throwMissing("Module Rotational Velocity"); if (optimalVoltage == -1) throwMissing("Optimal Voltage"); if (moveCurrentLimit == -1) throwMissing("Move Current Limit"); if (turnCurrentLimit == -1) throwMissing("Turn Current Limit"); if (moveGearRatio == -1) throwMissing("Move Gear Ratio"); if (turnGearRatio == -1) throwMissing("Turn Gear Ratio"); if (wheelDiameterInches == -1) throwMissing("Wheel Diameter"); - if (maxV == -1) throwMissing("Max Robot Velocity"); - if (maxRv == -1) throwMissing("Max Robot Rotational Velocity"); - if (maxA == -1) throwMissing("Max Robot Acceleration"); - if (maxModuleRv == -1) throwMissing("Max Module Rotational Velocity"); - if (moveMotorType == null) throwMissing("Move Motor Type"); - if (turnMotorType == null) throwMissing("Turn Motor Type"); if (discretizationLookahead == -1) throwMissing("Discretization Lookahead"); if (odometryPeriod == -1) throwMissing("Odometry Period"); - if (standardDeviations == null) throwMissing("Standard Deviations"); + if (odometryStd == null) throwMissing("Odometry Standard Deviations"); if (sysIdConfig == null) throwMissing("SysId Config"); if (fieldLength == -1) throwMissing("Field Length"); if (fieldWidth == -1) throwMissing("Field Width"); @@ -547,19 +533,19 @@ public void verify() { if ( ( - !turnMotorType.equals(SwerveMotorType.SPARK_MAX_BRUSHED) && - !turnMotorType.equals(SwerveMotorType.SPARK_MAX_BRUSHLESS) && - !turnMotorType.equals(SwerveMotorType.SPARK_FLEX_BRUSHED) && - !turnMotorType.equals(SwerveMotorType.SPARK_FLEX_BRUSHLESS) + !turnMotorType.equals(SwerveMotor.Type.SPARK_MAX_BRUSHED) && + !turnMotorType.equals(SwerveMotor.Type.SPARK_MAX_BRUSHLESS) && + !turnMotorType.equals(SwerveMotor.Type.SPARK_FLEX_BRUSHED) && + !turnMotorType.equals(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) ) && - module.getEncoderType().equals(SwerveEncoderType.SPARK_ENCODER) + module.getEncoderType().equals(SwerveEncoder.Type.SPARK_ENCODER) ) throw new UnsupportedOperationException("Cannot use Spark attached encoder on non-Spark motor"); if ( - !module.getMoveMotorCanBus().isEmpty() && !moveMotorType.equals(SwerveMotorType.TALONFX) + !module.getMoveMotorCanBus().isEmpty() && !moveMotorType.equals(SwerveMotor.Type.TALONFX) ) throw new UnsupportedOperationException("Cannot set custom CAN bus for non-Talon FX motor"); if ( - !module.getTurnMotorCanBus().isEmpty() && !turnMotorType.equals(SwerveMotorType.TALONFX) + !module.getTurnMotorCanBus().isEmpty() && !turnMotorType.equals(SwerveMotor.Type.TALONFX) ) throw new UnsupportedOperationException("Cannot set custom CAN bus for non-Talon FX motor"); } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java index e2751f3..73d1a6e 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveModuleConfig.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.geometry.Translation2d; import java.util.MissingResourceException; -import org.team340.lib.swerve.SwerveBase.SwerveEncoderType; +import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; /** * Config builder for {@link SwerveBase} modules. @@ -10,7 +10,7 @@ public class SwerveModuleConfig { private String label; - private SwerveEncoderType encoderType; + private SwerveEncoder.Type encoderType; private int encoderDeviceId = -1; private String encoderCanBus; private double encoderOffset = 0; @@ -61,7 +61,7 @@ public SwerveModuleConfig useCANcoder(int deviceId, double offset, boolean flipp * @param inverted If the encoder is inverted. */ public SwerveModuleConfig useCANcoder(int deviceId, String canBus, double offset, boolean inverted) { - encoderType = SwerveEncoderType.CANCODER; + encoderType = SwerveEncoder.Type.CANCODER; encoderDeviceId = deviceId; encoderCanBus = canBus; encoderOffset = offset; @@ -76,7 +76,7 @@ public SwerveModuleConfig useCANcoder(int deviceId, String canBus, double offset * @param inverted If the encoder is inverted. */ public SwerveModuleConfig useSparkAttachedEncoder(double offset, boolean inverted) { - encoderType = SwerveEncoderType.SPARK_ENCODER; + encoderType = SwerveEncoder.Type.SPARK_ENCODER; encoderOffset = offset; encoderInverted = inverted; return this; @@ -85,7 +85,7 @@ public SwerveModuleConfig useSparkAttachedEncoder(double offset, boolean inverte /** * Gets the selected absolute encoder's type. */ - public SwerveEncoderType getEncoderType() { + public SwerveEncoder.Type getEncoderType() { return encoderType; } @@ -254,7 +254,7 @@ public boolean getTurnMotorInverted() { public void verify() { if (label == null) throwMissing("Label"); if (encoderType == null) throwMissing("Encoder Type"); - if (encoderType.equals(SwerveEncoderType.CANCODER)) { + if (encoderType.equals(SwerveEncoder.Type.CANCODER)) { if (encoderDeviceId == -1) throwMissing("CANcoder Device ID"); if (encoderCanBus == null) throwMissing("CANcoder CAN Bus"); } diff --git a/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java b/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java index 6ecba2b..f057cbd 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java +++ b/src/main/java/org/team340/lib/swerve/hardware/encoders/SwerveEncoder.java @@ -1,10 +1,20 @@ package org.team340.lib.swerve.hardware.encoders; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; + /** * An absolute encoder wrapper for swerve. * Bound to {@link SwerveModule}s. */ public interface SwerveEncoder { + /** + * Supported encoders. + */ + public static enum Type { + CANCODER, + SPARK_ENCODER, + } + /** * Gets the encoder's position in radians. * Clamped from {@code -PI} to {@code PI}. diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java b/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java index d60e770..1046280 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java +++ b/src/main/java/org/team340/lib/swerve/hardware/imu/SwerveIMU.java @@ -6,6 +6,14 @@ * An IMU wrapper for swerve. */ public interface SwerveIMU { + /** + * Supported IMUs. + */ + public static enum Type { + ADIS16470, + PIGEON2, + } + /** * Gets the IMU's absolute yaw. */ diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java b/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java index ebfdca1..d4102ec 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java @@ -1,10 +1,23 @@ package org.team340.lib.swerve.hardware.motors; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; + /** * A motor for swerve, can be a move or turn motor. * Bound to {@link SwerveModule}s. */ public interface SwerveMotor { + /** + * Supported motors. + */ + public static enum Type { + SPARK_MAX_BRUSHED, + SPARK_MAX_BRUSHLESS, + SPARK_FLEX_BRUSHED, + SPARK_FLEX_BRUSHLESS, + TALONFX, + } + /** * Gets the motor's velocity. */ diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java index 383c6b6..4a6bf8a 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java @@ -3,10 +3,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkFlex; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; -import org.team340.lib.swerve.SwerveBase.SwerveEncoderType; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; @@ -57,7 +56,7 @@ public SwerveSparkFlex( int periodMs = (int) (config.getPeriod() * 1000.0); int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0); - boolean usingAttachedEncoder = SwerveEncoderType.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); + boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); @@ -97,7 +96,7 @@ public SwerveSparkFlex( .setVelocityConversionFactor(Math2.TWO_PI / 60.0) .setInverted(moduleConfig.getEncoderInverted()) .setZeroOffset(moduleConfig.getEncoderOffset()) - .apply(sparkFlex, sparkFlex.getAbsoluteEncoder(Type.kDutyCycle)); + .apply(sparkFlex, sparkFlex.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)); } sparkFlex.set(0.0); diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java index 2d6e0fa..fa20b04 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java @@ -3,10 +3,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController.ArbFFUnits; -import org.team340.lib.swerve.SwerveBase.SwerveEncoderType; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; @@ -57,7 +56,7 @@ public SwerveSparkMax( int periodMs = (int) (config.getPeriod() * 1000.0); int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0); - boolean usingAttachedEncoder = SwerveEncoderType.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); + boolean usingAttachedEncoder = SwerveEncoder.Type.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); @@ -97,7 +96,7 @@ public SwerveSparkMax( .setVelocityConversionFactor(Math2.TWO_PI / 60.0) .setInverted(moduleConfig.getEncoderInverted()) .setZeroOffset(moduleConfig.getEncoderOffset()) - .apply(sparkMax, sparkMax.getAbsoluteEncoder(Type.kDutyCycle)); + .apply(sparkMax, sparkMax.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle)); } sparkMax.set(0.0); diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java b/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java index 46937d1..4f35435 100644 --- a/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java +++ b/src/main/java/org/team340/lib/swerve/util/SwerveField2d.java @@ -2,13 +2,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import java.util.ArrayList; -import java.util.List; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.util.Math2; @@ -18,24 +14,21 @@ public class SwerveField2d extends Field2d { private final SwerveConfig config; - private final Translation2d[] moduleTranslations; /** * Create the field. * @param config The general swerve config. * @param moduleTranslations Translations representing the locations of the swerve modules. */ - public SwerveField2d(SwerveConfig config, Translation2d[] moduleTranslations) { + public SwerveField2d(SwerveConfig config) { this.config = config; - this.moduleTranslations = moduleTranslations; } /** * Update the field. * @param newPose The robot's new pose. - * @param modulePositions The robot's module positions. */ - public void update(Pose2d newPose, SwerveModulePosition[] modulePositions) { + public void update(Pose2d newPose) { if (!DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue)) { newPose = new Pose2d( @@ -52,19 +45,5 @@ public void update(Pose2d newPose, SwerveModulePosition[] modulePositions) { Rotation2d.fromDegrees(Math2.toFixed(newPose.getRotation().getDegrees())) ) ); - - List modulePoses = new ArrayList<>(); - for (int i = 0; i < moduleTranslations.length; i++) { - Translation2d translation = moduleTranslations[i].rotateBy(newPose.getRotation()).plus(newPose.getTranslation()); - modulePoses.add( - new Pose2d( - Math2.toFixed(translation.getX()), - Math2.toFixed(translation.getY()), - Rotation2d.fromDegrees(Math2.toFixed(modulePositions[i].angle.plus(newPose.getRotation()).getDegrees())) - ) - ); - } - - getObject("Modules").setPoses(modulePoses); } } diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java b/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java new file mode 100644 index 0000000..0f0d9de --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/util/SwerveOdometryThread.java @@ -0,0 +1,126 @@ +package org.team340.lib.swerve.util; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayDeque; +import java.util.Deque; +import java.util.Iterator; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Supplier; +import org.team340.lib.swerve.SwerveModule; +import org.team340.lib.swerve.config.SwerveConfig; + +/** + * Samples swerve hardware on a separate thread running at a higher frequency than the main robot loop to provide more accurate odometry. + * Call {@link SwerveOdometryThread#update(SwerveDrivePoseEstimator)} to update the pose estimator with sampled measurements. + */ +public class SwerveOdometryThread { + + private record ModuleQueues(Deque distance, Deque heading) {} + + private final SwerveModule[] modules; + private final Supplier yawSupplier; + + private final ModuleQueues[] moduleQueues; + private final Deque yawQueue = new ArrayDeque<>(); + private final Deque timestampQueue = new ArrayDeque<>(); + + private final Notifier thread; + private final double period; + private final ReentrantLock mutex = new ReentrantLock(); + + /** + * Creates the odometry thread. + * @param modules The robot's swerve modules. + * @param yawSupplier A supplier for the robot's yaw. + */ + public SwerveOdometryThread(SwerveModule[] modules, Supplier yawSupplier, SwerveConfig config) { + this.modules = modules; + this.yawSupplier = yawSupplier; + + moduleQueues = new ModuleQueues[modules.length]; + for (int i = 0; i < modules.length; i++) { + moduleQueues[i] = new ModuleQueues(new ArrayDeque<>(), new ArrayDeque<>()); + } + + thread = new Notifier(this::sample); + thread.setName("Swerve Odometry"); + period = config.getOdometryPeriod(); + } + + /** + * Starts the thread. + */ + public void start() { + thread.startPeriodic(period); + } + + /** + * Updates the pose estimator with recorded odometry samples. + * @param poseEstimator The pose estimator to update. + */ + public void update(SwerveDrivePoseEstimator poseEstimator) { + try { + mutex.lock(); + Iterator timestampIterator = timestampQueue.iterator(); + while (timestampIterator.hasNext()) { + double timestamp = timestampIterator.next(); + + boolean missingModule = false; + SwerveModulePosition[] modulePositions = new SwerveModulePosition[moduleQueues.length]; + for (int i = 0; i < modulePositions.length; i++) { + double distance, heading; + try { + distance = moduleQueues[i].distance().pop(); + heading = moduleQueues[i].heading().pop(); + } catch (Exception e) { + missingModule = true; + continue; + } + + modulePositions[i] = new SwerveModulePosition(distance, new Rotation2d(heading)); + } + + if (missingModule) continue; + + try { + Rotation2d yaw = yawQueue.pop(); + poseEstimator.updateWithTime(timestamp, yaw, modulePositions); + } catch (Exception e) { + continue; + } + } + } finally { + yawQueue.clear(); + timestampQueue.clear(); + for (ModuleQueues queues : moduleQueues) { + queues.distance().clear(); + queues.heading().clear(); + } + + mutex.unlock(); + } + } + + /** + * Samples hardware. + */ + private void sample() { + try { + mutex.lock(); + timestampQueue.add(MathSharedStore.getTimestamp()); + yawQueue.add(yawSupplier.get()); + for (int i = 0; i < modules.length; i++) { + SwerveModule module = modules[i]; + ModuleQueues queues = moduleQueues[i]; + queues.distance().add(module.getDistance()); + queues.heading().add(module.getHeading()); + } + } finally { + mutex.unlock(); + } + } +} diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java b/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java index 7598bd9..ac4c3d4 100644 --- a/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java +++ b/src/main/java/org/team340/lib/swerve/util/SwerveRatelimiter.java @@ -76,7 +76,7 @@ public SwerveState calculate(ChassisSpeeds desiredSpeeds) { SwerveModuleState[] desiredModuleStates = kinematics.toSwerveModuleStates(desiredSpeeds); // - Preliminary wheel speed desaturation based on the configured max robot velocity. - SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, config.getMaxV()); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, config.getVelocity()); // - Determine desired speeds based on desaturated module states. desiredSpeeds = kinematics.toChassisSpeeds(desiredModuleStates); @@ -185,7 +185,7 @@ public SwerveState calculate(ChassisSpeeds desiredSpeeds) { // - Find the maximum feasible delta in the a module's heading. // Acceleration is assumed to be a non-factor. - double maxHeadingDelta = config.getPeriod() * config.getMaxModuleRv(); + double maxHeadingDelta = config.getPeriod() * config.getModuleRotationalVelocity(); // (Limit the velocity delta based on module heading constraints) // If we aren't stopping: @@ -282,7 +282,7 @@ public SwerveState calculate(ChassisSpeeds desiredSpeeds) { modulesLastVy[i], desiredModuleVx, desiredModuleVy, - config.getPeriod() * config.getMaxA() + config.getPeriod() * config.getAcceleration() ); // - Set the scalar to the minimum of its current value and the diff --git a/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java b/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java new file mode 100644 index 0000000..6e19ab4 --- /dev/null +++ b/src/main/java/org/team340/lib/swerve/util/SwerveSerializer.java @@ -0,0 +1,70 @@ +package org.team340.lib.swerve.util; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.team340.lib.util.Math2; + +/** + * Serializers for swerve helper classes. + */ +public final class SwerveSerializer { + + private SwerveSerializer() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Returns the norm of the provided chassis speeds in meters/second. + * @param speeds The chassis speeds. + */ + public static double chassisSpeedsNorm(ChassisSpeeds speeds) { + return chassisSpeedsNorm(speeds, false); + } + + /** + * Returns the norm of the provided chassis speeds in meters/second. + * @param speeds The chassis speeds. + * @param toFixed If the returned value should be rounded to a fixed precision. + */ + public static double chassisSpeedsNorm(ChassisSpeeds speeds, boolean toFixed) { + return Math2.toFixed(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); + } + + /** + * Converts module states into a single array. This is made to be compatible with AdvantageScope. + * Returns in the following format (heading is in radians): + * 
+     * [
+     *   heading0, velocity0,
+     *   heading1, velocity1,
+     *   ...
+     * ]
+     *
+ * @param states The states to convert. + */ + public static double[] moduleStatesDoubleArray(SwerveModuleState[] states) { + return moduleStatesDoubleArray(states, false); + } + + /** + * Converts module states into a single array. This is made to be compatible with AdvantageScope. + * Returns in the following format (heading is in radians): + * 
+     * [
+     *   heading0, velocity0,
+     *   heading1, velocity1,
+     *   ...
+     * ]
+     *
+ * @param states The states to convert. + * @param toFixed If the returned values should be rounded to a fixed precision. + */ + public static double[] moduleStatesDoubleArray(SwerveModuleState[] states, boolean toFixed) { + double[] array = new double[states.length * 2]; + for (int i = 0; i < states.length; i++) { + array[i * 2] = states[i].angle.getRadians(); + array[(i * 2) + 1] = states[i].speedMetersPerSecond; + } + return array; + } +} diff --git a/src/main/java/org/team340/lib/util/Math2.java b/src/main/java/org/team340/lib/util/Math2.java index ebb7b2c..82a4429 100644 --- a/src/main/java/org/team340/lib/util/Math2.java +++ b/src/main/java/org/team340/lib/util/Math2.java @@ -26,6 +26,10 @@ private Math2() { * Shared maximum accuracy floating point. */ public static final double DEFAULT_EPSILON = 1e-9; + /** + * Default precision used in {@link Math2#toFixed(double)}. + */ + public static final double DEFAULT_TO_FIXED_PRECISION = 1e-3; /** * {@code PI * 2} */ @@ -159,7 +163,7 @@ public static boolean twist2dEpsilonEquals(Twist2d a, Twist2d b, double epsilon) * @return The rounded value. */ public static double toFixed(double value) { - return toFixed(value, 1e-3); + return toFixed(value, DEFAULT_TO_FIXED_PRECISION); } /** diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java index 38a7996..048ac9a 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java @@ -341,7 +341,7 @@ public SparkFlexConfig follow(CANSparkFlex.ExternalFollower leader, int deviceId /** * Sets timeout for sending CAN messages with {@code setParameter*()} and {@code getParameter*()} * calls.These calls will block for up to this amount of time before returning a timeout error. - * A timeout of {@code 0.0} will make the {@code setParameter*()} calls non-blocking, and instead + * A timeout of {@code 0} will make the {@code setParameter*()} calls non-blocking, and instead * will check the response in a separate thread. With this configuration, any error messages will * appear on the driver station but will not be returned by the {@code getLastError()} call. * @param milliseconds The timeout in milliseconds. @@ -424,7 +424,7 @@ public SparkFlexConfig setPeriodicFramePeriod(Frame frame, int periodMs) { * 'on/off' controller. This limit is enabled by default but is set higher than * the default Smart Current Limit. The time the controller is off after the current * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0.0} which is + * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is * the minimum time and is part of a PWM cycle from when the over current is detected. * This allows the controller to regulate the current close to the limit value. * @param limit The current limit in amps. @@ -441,7 +441,7 @@ public SparkFlexConfig setSecondaryCurrentLimit(double limit) { * 'on/off' controller. This limit is enabled by default but is set higher than * the default Smart Current Limit. The time the controller is off after the current * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0.0} which is + * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is * the minimum time and is part of a PWM cycle from when the over current is detected. * This allows the controller to regulate the current close to the limit value. * @param limit The current limit in amps. @@ -478,8 +478,8 @@ public SparkFlexConfig setSmartCurrentLimit(int limit) { * smarter strategy to deal with high current draws and keep the motor and controller * operating in a safe region. The controller can also limit the current based on the RPM * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0.0}. - * @param stallLimit The current limit in amps at {@code 0.0} RPM. + * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. + * @param stallLimit The current limit in amps at {@code 0} RPM. * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). */ public SparkFlexConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { @@ -497,8 +497,8 @@ public SparkFlexConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { * smarter strategy to deal with high current draws and keep the motor and controller * operating in a safe region. The controller can also limit the current based on the RPM * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0.0}. - * @param stallLimit The current limit in amps at {@code 0.0} RPM. + * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. + * @param stallLimit The current limit in amps at {@code 0} RPM. * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). * @param limitRPM RPM less than this value will be set to the stallLimit, RPM values greater than limitRPM will scale linearly to freeLimit. */ diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java index 34c3e30..9af895a 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java @@ -341,7 +341,7 @@ public SparkMaxConfig follow(CANSparkMax.ExternalFollower leader, int deviceId, /** * Sets timeout for sending CAN messages with {@code setParameter*()} and {@code getParameter*()} * calls.These calls will block for up to this amount of time before returning a timeout error. - * A timeout of {@code 0.0} will make the {@code setParameter*()} calls non-blocking, and instead + * A timeout of {@code 0} will make the {@code setParameter*()} calls non-blocking, and instead * will check the response in a separate thread. With this configuration, any error messages will * appear on the driver station but will not be returned by the {@code getLastError()} call. * @param milliseconds The timeout in milliseconds. @@ -424,7 +424,7 @@ public SparkMaxConfig setPeriodicFramePeriod(Frame frame, int periodMs) { * 'on/off' controller. This limit is enabled by default but is set higher than * the default Smart Current Limit. The time the controller is off after the current * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0.0} which is + * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is * the minimum time and is part of a PWM cycle from when the over current is detected. * This allows the controller to regulate the current close to the limit value. * @param limit The current limit in amps. @@ -441,7 +441,7 @@ public SparkMaxConfig setSecondaryCurrentLimit(double limit) { * 'on/off' controller. This limit is enabled by default but is set higher than * the default Smart Current Limit. The time the controller is off after the current * limit is reached is determined by the parameter limitCycles, which is the number - * of PWM cycles (20kHz). The recommended value is the default of {@code 0.0} which is + * of PWM cycles (20kHz). The recommended value is the default of {@code 0} which is * the minimum time and is part of a PWM cycle from when the over current is detected. * This allows the controller to regulate the current close to the limit value. * @param limit The current limit in amps. @@ -478,8 +478,8 @@ public SparkMaxConfig setSmartCurrentLimit(int limit) { * smarter strategy to deal with high current draws and keep the motor and controller * operating in a safe region. The controller can also limit the current based on the RPM * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0.0}. - * @param stallLimit The current limit in amps at {@code 0.0} RPM. + * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. + * @param stallLimit The current limit in amps at {@code 0} RPM. * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). */ public SparkMaxConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { @@ -497,8 +497,8 @@ public SparkMaxConfig setSmartCurrentLimit(int stallLimit, int freeLimit) { * smarter strategy to deal with high current draws and keep the motor and controller * operating in a safe region. The controller can also limit the current based on the RPM * of the motor in a linear fashion to help with controllability in closed loop control. - * For a response that is linear the entire RPM range leave limit RPM at {@code 0.0}. - * @param stallLimit The current limit in amps at {@code 0.0} RPM. + * For a response that is linear the entire RPM range leave limit RPM at {@code 0}. + * @param stallLimit The current limit in amps at {@code 0} RPM. * @param freeLimit The current limit at free speed ({@code 5700} RPM for NEO). * @param limitRPM RPM less than this value will be set to the stallLimit, RPM values greater than limitRPM will scale linearly to freeLimit. */ diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java index 847399e..21b0023 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java @@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.AccelStrategy; import org.team340.lib.util.Math2; import org.team340.lib.util.config.PIDConfig; import org.team340.lib.util.config.PIDFConfig; @@ -87,7 +88,7 @@ public SparkPIDControllerConfig setD(double gain) { /** * Sets the derivative gain constant of the PIDF controller on the Spark Max. * @param gain The derivative gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setD(double gain, int slotId) { addStep( @@ -114,7 +115,7 @@ public SparkPIDControllerConfig setDFilter(double gain) { /** * Sets the the derivative filter constant of the PIDF controller on the Spark Max. * @param gain The derivative filter value, must be a positive number between {@code 0.0} and {@code 1.0}. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setDFilter(double gain, int slotId) { addStep( @@ -155,7 +156,7 @@ public SparkPIDControllerConfig setFF(double gain) { /** * Sets the feed-forward gain constant of the PIDF controller on the Spark Max. * @param gain The feed-forward gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setFF(double gain, int slotId) { addStep( @@ -182,7 +183,7 @@ public SparkPIDControllerConfig setI(double gain) { /** * Sets the integral gain constant of the PIDF controller on the Spark Max. * @param gain The integral gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setI(double gain, int slotId) { addStep( @@ -193,6 +194,21 @@ public SparkPIDControllerConfig setI(double gain, int slotId) { return this; } + /** + * Configure the maximum I accumulator of the PID controller. This value is used to constrain the + * I accumulator to help manage integral wind-up + * @param iMaxAccum The max value to constrain the I accumulator to. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setIMaxAccum(double iMaxAccum, int slotId) { + addStep( + pidController -> pidController.setIMaxAccum(iMaxAccum, slotId), + pidController -> Math2.epsilonEquals(pidController.getIMaxAccum(slotId), iMaxAccum, RevConfigUtils.EPSILON), + "I Max Accumulator (Slot " + slotId + ")" + ); + return this; + } + /** * Sets the IZone range of the PIDF controller on the Spark Max. This value specifies the * range the error must be within for the integral constant to take effect. @@ -211,7 +227,7 @@ public SparkPIDControllerConfig setIZone(double iZone) { * Sets the IZone range of the PIDF controller on the Spark Max. This value specifies the * range the error must be within for the integral constant to take effect. * @param iZone The I zone value, must be positive. Set to {@code 0.0} to disable. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setIZone(double iZone, int slotId) { addStep( @@ -242,7 +258,7 @@ public SparkPIDControllerConfig setOutputRange(double min, double max) { * Sets the min amd max output for the closed loop mode. * @param min Reverse power minimum to allow the controller to output. * @param max Forward power maximum to allow the controller to output. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setOutputRange(double min, double max, int slotId) { addStep( @@ -271,7 +287,7 @@ public SparkPIDControllerConfig setP(double gain) { /** * Sets the proportional gain constant of the PIDF controller on the Spark Max. * @param gain The proportional gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setP(double gain, int slotId) { addStep( @@ -350,7 +366,7 @@ public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain) * @param pGain The proportional gain value, must be positive. * @param iGain The integral gain value, must be positive. * @param dGain The derivative gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain, int slotId) { setP(pGain, slotId); @@ -372,7 +388,7 @@ public SparkPIDControllerConfig setPID(PIDConfig pidfConfig) { /** * Sets PIDF gains on the Spark Max. * @param pidConfig The PID config object to apply. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setPID(PIDConfig pidfConfig, int slotId) { setPID(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), slotId); @@ -401,7 +417,7 @@ public SparkPIDControllerConfig setPIDF(double pGain, double iGain, double dGain * @param iGain The integral gain value, must be positive. * @param dGain The derivative gain value, must be positive. * @param ffGain The feed-forward gain value, must be positive. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setPIDF(double pGain, double iGain, double dGain, double ffGain, int slotId) { setP(pGain, slotId); @@ -424,11 +440,85 @@ public SparkPIDControllerConfig setPIDF(PIDFConfig pidfConfig) { /** * Sets PIDF gains on the Spark Max. * @param pidfConfig The PIDF config object to apply. - * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. */ public SparkPIDControllerConfig setPIDF(PIDFConfig pidfConfig, int slotId) { setPIDF(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), pidfConfig.ff(), slotId); setIZone(pidfConfig.iZone(), slotId); return this; } + + /** + * Configure the acceleration strategy used to control acceleration on the motor. + * @param accelStrategy The acceleration strategy to use for the automatically generated motion profile. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setSmartMotionAccelStrategy(AccelStrategy accelStrategy, int slotId) { + addStep( + pidController -> pidController.setSmartMotionAccelStrategy(accelStrategy, slotId), + pidController -> pidController.getSmartMotionAccelStrategy(slotId).equals(accelStrategy), + "Smart Motion Acceleration Strategy (Slot " + slotId + ")" + ); + return this; + } + + /** + * Configure the allowed closed loop error of SmartMotion mode. This value is how much deviation + * from your setpoint is tolerated and is useful in preventing oscillation around your setpoint. + * @param allowedErr The allowed deviation for your setpoint vs actual position in rotations. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setSmartMotionAllowedClosedLoopError(double allowedErr, int slotId) { + addStep( + pidController -> pidController.setSmartMotionAllowedClosedLoopError(allowedErr, slotId), + pidController -> + Math2.epsilonEquals(pidController.getSmartMotionAllowedClosedLoopError(slotId), allowedErr, RevConfigUtils.EPSILON), + "Smart Motion Allowed Closed Loop Error (Slot " + slotId + ")" + ); + return this; + } + + /** + * Configure the maximum acceleration of the SmartMotion mode. This is the acceleration that the + * motor velocity will increase at until the max velocity is reached + * @param maxAccel The maximum acceleration for the motion profile in RPM per second. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setSmartMotionMaxAccel(double maxAccel, int slotId) { + addStep( + pidController -> pidController.setSmartMotionMaxAccel(maxAccel, slotId), + pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxAccel(slotId), maxAccel, RevConfigUtils.EPSILON), + "Smart Motion Max Acceleration (Slot " + slotId + ")" + ); + return this; + } + + /** + * Configure the maximum velocity of the SmartMotion mode. This is the velocity that is reached in + * the middle of the profile and is what the motor should spend most of its time at. + * @param maxVel The maximum cruise velocity for the motion profile in RPM. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setSmartMotionMaxVelocity(double maxVel, int slotId) { + addStep( + pidController -> pidController.setSmartMotionMaxVelocity(maxVel, slotId), + pidController -> Math2.epsilonEquals(pidController.getSmartMotionMaxVelocity(slotId), maxVel, RevConfigUtils.EPSILON), + "Smart Motion Max Velocity (Slot " + slotId + ")" + ); + return this; + } + + /** + * Configure the minimum velocity of the SmartMotion mode. Any requested velocities below this value will be set to {@code 0.0}. + * @param minVel The minimum velocity for the motion profile in RPM. + * @param slotId The gain schedule slot, the value is a number between {@code 0} and {@code 3}. + */ + public SparkPIDControllerConfig setSmartMotionMinOutputVelocity(double minVel, int slotId) { + addStep( + pidController -> pidController.setSmartMotionMinOutputVelocity(minVel, slotId), + pidController -> Math2.epsilonEquals(pidController.getSmartMotionMinOutputVelocity(slotId), minVel, RevConfigUtils.EPSILON), + "Smart Motion Min Velocity (Slot " + slotId + ")" + ); + return this; + } } diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index ec0a36d..2c98091 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -8,9 +8,9 @@ import edu.wpi.first.wpilibj.SPI.Port; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.team340.lib.controller.Controller2Config; -import org.team340.lib.swerve.SwerveBase.SwerveMotorType; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; +import org.team340.lib.swerve.hardware.motors.SwerveMotor; import org.team340.lib.util.Math2; import org.team340.lib.util.config.PIDConfig; import org.team340.lib.util.config.rev.AbsoluteEncoderConfig; @@ -142,6 +142,7 @@ public static final class IntakeConstants { } public static final class ShooterConstants { + private static final SparkFlexConfig SHOOT_MOTOR_BASE_CONFIG = new SparkFlexConfig() .clearFaults() .restoreFactoryDefaults() @@ -163,16 +164,17 @@ public static final class ShooterConstants { .setClosedLoopRampRate(1.5) .setOpenLoopRampRate(1.5); - public static final SparkPIDControllerConfig FEED_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); - public static final SparkPIDControllerConfig SHOOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); + public static final SparkPIDControllerConfig FEED_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); + public static final SparkPIDControllerConfig SHOOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); - public static final double FEED_INTAKE_SPEED = 0.0; - } + public static final double FEED_INTAKE_SPEED = 0.0; + } /** * All distances in inches. All angles in radians. */ public static final class PivotConstants { + public static final SparkMaxConfig PIVOT_MOTOR_CONFIG = new SparkMaxConfig() .clearFaults() .restoreFactoryDefaults() @@ -181,7 +183,7 @@ public static final class PivotConstants { .setIdleMode(IdleMode.kCoast) .setClosedLoopRampRate(1.5) .setOpenLoopRampRate(1.5); - + public static final SparkPIDControllerConfig PIVOT_PID_CONFIG = new SparkPIDControllerConfig().setPID(0.0, 0.0, 0.0, 0); public static final double PID_MIN_OUTPUT = 0.0; @@ -207,7 +209,6 @@ public static final class PivotConstants { public static final double MAXIMUM_ANGLE = 0.0; public static final double HOMING_SPEED = 0.0; - } /** @@ -250,13 +251,14 @@ public static final class SwerveConstants { .setMoveFF(0.0, 2.84, 0.0) .setTurnPID(0.5, 0.0, 15.0, 0.0) .setRampRate(0.03, 0.03) + .setMotorTypes(SwerveMotor.Type.SPARK_FLEX_BRUSHLESS, SwerveMotor.Type.SPARK_FLEX_BRUSHLESS) + .setMaxSpeeds(5.0, 10.0) + .setRatelimits(17.5, 30.0) .setPowerProperties(VOLTAGE, 60.0, 30.0) .setMechanicalProperties(6.5, 7.0, 4.0) - .setSpeedConstraints(5.0, 10.0, 17.5, 30.0) - .setMotorTypes(SwerveMotorType.SPARK_FLEX_BRUSHLESS, SwerveMotorType.SPARK_FLEX_BRUSHLESS) .setDiscretizationLookahead(0.020) - .setOdometryPeriod(0.004) - .setStandardDeviations(0.1, 0.1, 0.1) + .setOdometryPeriod(0.005) + .setOdometryStd(0.1, 0.1, 0.1) .setSysIdConfig(new SysIdRoutine.Config()) .setFieldSize(FIELD_LENGTH, FIELD_WIDTH) .addModule(FRONT_LEFT) diff --git a/src/main/java/org/team340/robot/subsystems/Pivot.java b/src/main/java/org/team340/robot/subsystems/Pivot.java index 95c9eb9..4b8c54d 100644 --- a/src/main/java/org/team340/robot/subsystems/Pivot.java +++ b/src/main/java/org/team340/robot/subsystems/Pivot.java @@ -1,19 +1,16 @@ package org.team340.robot.subsystems; -import org.team340.lib.GRRSubsystem; -import org.team340.robot.Constants.PivotConstants; -import org.team340.robot.Constants.RobotMap; - import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; - import com.revrobotics.CANSparkMax; import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import org.team340.lib.GRRSubsystem; +import org.team340.robot.Constants.PivotConstants; +import org.team340.robot.Constants.RobotMap; public class Pivot extends GRRSubsystem { + private final CANSparkMax pivotMotor = createSparkMax("Pivot Motor", RobotMap.SHOOTER_PIVOT_MOTOR, MotorType.kBrushless); private final SparkPIDController pivotPID = pivotMotor.getPIDController(); private final DigitalInput lowerLimit = new DigitalInput(RobotMap.PIVOT_LOWER_LIMIT); @@ -22,7 +19,6 @@ public class Pivot extends GRRSubsystem { public Pivot() { super("Pivot"); - PivotConstants.PIVOT_MOTOR_CONFIG.apply(pivotMotor); PivotConstants.PIVOT_PID_CONFIG.apply(pivotMotor, pivotPID); @@ -42,14 +38,14 @@ public Pivot() { public Command home(boolean withOverride) { return commandBuilder("pivot.home()") .onExecute(() -> { - if(!hasBeenHomed || withOverride) { + if (!hasBeenHomed || withOverride) { pivotMotor.set(PivotConstants.HOMING_SPEED); } else { pivotMotor.stopMotor(); } }) .isFinished(() -> lowerLimit.get() || (hasBeenHomed && !withOverride)) - .onEnd((interrupted) -> { + .onEnd(interrupted -> { pivotMotor.stopMotor(); hasBeenHomed = !interrupted; }); diff --git a/src/main/java/org/team340/robot/subsystems/Shooter.java b/src/main/java/org/team340/robot/subsystems/Shooter.java index 11af6f5..039280d 100644 --- a/src/main/java/org/team340/robot/subsystems/Shooter.java +++ b/src/main/java/org/team340/robot/subsystems/Shooter.java @@ -1,12 +1,9 @@ package org.team340.robot.subsystems; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DigitalInput; @@ -14,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.Command; import java.util.function.Supplier; import org.team340.lib.GRRSubsystem; -import org.team340.lib.util.Math2; import org.team340.lib.util.Mutable; import org.team340.lib.util.config.rev.SparkPIDControllerConfig; import org.team340.robot.Constants.RobotMap; import org.team340.robot.Constants.ShooterConstants; public class Shooter extends GRRSubsystem { + private final CANSparkMax feedMotor = createSparkMax("Feeder Motor", RobotMap.SHOOTER_FEED_MOTOR, MotorType.kBrushless); private final CANSparkFlex leftShootMotor = createSparkFlex( "Shooter Motor Left", @@ -41,7 +38,6 @@ public class Shooter extends GRRSubsystem { public Shooter() { super("Shooter"); - new SparkPIDControllerConfig() .setPID(ShooterConstants.FEED_PID.p(), ShooterConstants.FEED_PID.i(), ShooterConstants.FEED_PID.d()) .setIZone(ShooterConstants.FEED_PID.iZone())