The coordinate system for the gyro interface is the same as the Pigeon 2.0's coordinate + * system: https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf + * *
The Gyro is modeled not as a subsystem but rather a shared resource that may be used by * multiple subsystems. Since it doesn't have a periodic method like a subsystem, it is important * that its updateInputs method is called just once via another periodic method. (This requires some * coordination, and, in this library, it is invoked via the drivetrain subsystem's periodic - * mehtod.) + * method.) * *
There is not a simulated version of this interface. Instead, the drivetrain supports
- * determining the robot's rotation from the gryo when connected and via the swwerve module
- * positions when not connected.
+ * determining the robot's rotation from the gyro when connected and via the swerve module positions
+ * when not connected.
*/
public interface GyroIO {
@AutoLog
public static class GyroIOInputs {
public boolean connected = false;
- public double positionDeg = 0.0;
- public double velocityDegPerSec = 0.0;
+ public double yawDeg = 0.0;
+ public double yawDegPerSec = 0.0;
+ public double pitchDeg = 0.0;
+ public double pitchDegPerSec = 0.0;
+ public double rollDeg = 0.0;
+ public double rollDegPerSec = 0.0;
}
/**
diff --git a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2.java b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2.java
index 1fca73bc..b87a2b08 100644
--- a/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2.java
+++ b/src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2.java
@@ -6,6 +6,7 @@
import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.sensors.Pigeon2;
+import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import frc.lib.team3061.RobotConfig;
public class GyroIOPigeon2 implements GyroIO {
@@ -14,13 +15,18 @@ public class GyroIOPigeon2 implements GyroIO {
public GyroIOPigeon2(int id) {
gyro = new Pigeon2(id, RobotConfig.getInstance().getCANBusName());
+ this.gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 9);
}
@Override
public void updateInputs(GyroIOInputs inputs) {
gyro.getRawGyro(xyzDps);
inputs.connected = gyro.getLastError().equals(ErrorCode.OK);
- inputs.positionDeg = gyro.getYaw(); // degrees
- inputs.velocityDegPerSec = xyzDps[2]; // degrees per second
+ inputs.yawDeg = gyro.getYaw(); // degrees
+ inputs.pitchDeg = gyro.getPitch(); // degrees
+ inputs.rollDeg = gyro.getRoll(); // degrees
+ inputs.rollDegPerSec = xyzDps[0]; // degrees per second
+ inputs.pitchDegPerSec = xyzDps[1]; // degrees per second
+ inputs.yawDegPerSec = xyzDps[2]; // degrees per second
}
}
diff --git a/src/main/java/frc/lib/team3061/swerve/Conversions.java b/src/main/java/frc/lib/team3061/swerve/Conversions.java
index 174fbe2d..8bb29c8e 100644
--- a/src/main/java/frc/lib/team3061/swerve/Conversions.java
+++ b/src/main/java/frc/lib/team3061/swerve/Conversions.java
@@ -57,13 +57,25 @@ public static double falconToMeters(double counts, double circumference, double
}
/**
- * @param velocitycounts Falcon Velocity Counts
+ * @param meters meters
+ * @param circumference Circumference of Wheel
+ * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
+ * @return Falcon Counts
+ */
+ public static double metersToFalcon(double meters, double circumference, double gearRatio) {
+ double wheelRotations = meters / circumference;
+ double motorRotations = wheelRotations * gearRatio;
+ return motorRotations * 2048.0;
+ }
+
+ /**
+ * @param velocityCounts Falcon Velocity Counts
* @param circumference Circumference of Wheel
* @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
* @return Falcon Velocity Counts
*/
- public static double falconToMPS(double velocitycounts, double circumference, double gearRatio) {
- double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+ public static double falconToMPS(double velocityCounts, double circumference, double gearRatio) {
+ double wheelRPM = falconToRPM(velocityCounts, gearRatio);
return (wheelRPM * circumference) / 60;
}
diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java
index bb54bcd9..7f233c9f 100644
--- a/src/main/java/frc/lib/team3061/swerve/SwerveModule.java
+++ b/src/main/java/frc/lib/team3061/swerve/SwerveModule.java
@@ -131,6 +131,15 @@ public int getModuleNumber() {
return moduleNumber;
}
+ /**
+ * Get the stator current of the drive motor of this swerve module.
+ *
+ * @return the stator current of the drive motor of this swerve module
+ */
+ public double getDriveCurrent() {
+ return inputs.driveCurrentAmps[0];
+ }
+
/**
* Update this swerve module's inputs and log them.
*
diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java b/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java
index f23ac823..f0430b1d 100644
--- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java
+++ b/src/main/java/frc/lib/team3061/swerve/SwerveModuleConstants.java
@@ -20,7 +20,11 @@ public enum SwerveType {
}
/* MK4i L2 */
- public static final double MK4I_L2_WHEEL_DIAMETER_METERS = 0.10033;
+
+ /*
+ Wheel diameter is best determined empirically. Refer to this document for more information: !!!
+ */
+ public static final double MK4I_L2_WHEEL_DIAMETER_METERS = 0.096294;
public static final double MK4I_L2_WHEEL_CIRCUMFERENCE = MK4I_L2_WHEEL_DIAMETER_METERS * Math.PI;
public static final double MK4I_L2_DRIVE_GEAR_RATIO =
1 / ((14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0));
@@ -30,6 +34,10 @@ public enum SwerveType {
public static final boolean MK4I_L2_CAN_CODER_INVERTED = false;
/* MK4 L2 */
+
+ /*
+ Wheel diameter is best determined empirically. Refer to this document for more information: !!!
+ */
public static final double MK4_L2_WHEEL_DIAMETER_METERS = 0.10033;
public static final double MK4_L2_WHEEL_CIRCUMFERENCE = MK4_L2_WHEEL_DIAMETER_METERS * Math.PI;
public static final double MK4_L2_DRIVE_GEAR_RATIO =
diff --git a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFX.java b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFX.java
index 88d0a6e7..5996e32a 100644
--- a/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFX.java
+++ b/src/main/java/frc/lib/team3061/swerve/SwerveModuleIOTalonFX.java
@@ -16,13 +16,11 @@
import edu.wpi.first.math.geometry.Rotation2d;
import frc.lib.team254.drivers.TalonFXFactory;
import frc.lib.team3061.RobotConfig;
-import frc.lib.team3061.util.CANDeviceFinder;
-import frc.lib.team3061.util.CANDeviceId.CANDeviceType;
import frc.lib.team6328.util.TunableNumber;
/**
* Implementation of the SwerveModuleIO interface for MK4 Swerve Modules with two Falcon 500 motors
- * and a CAN coder.
+ * and a CANcoder.
*/
public class SwerveModuleIOTalonFX implements SwerveModuleIO {
@@ -92,11 +90,6 @@ public SwerveModuleIOTalonFX(
RobotConfig.getInstance().getDriveKV() / 12,
RobotConfig.getInstance().getDriveKA() / 12);
- CANDeviceFinder can = new CANDeviceFinder();
- can.isDevicePresent(CANDeviceType.TALON, driveMotorID, "Mod " + moduleNumber + "Drive");
- can.isDevicePresent(CANDeviceType.TALON, angleMotorID, "Mod " + moduleNumber + "Angle");
- // check for the CANcoder on the CAN bus when supported by CANDeviceFinder
-
configAngleEncoder(canCoderID);
configAngleMotor(angleMotorID);
configDriveMotor(driveMotorID);
diff --git a/src/main/java/frc/lib/team3061/util/CANDeviceFinder.java b/src/main/java/frc/lib/team3061/util/CANDeviceFinder.java
deleted file mode 100644
index c58c0c97..00000000
--- a/src/main/java/frc/lib/team3061/util/CANDeviceFinder.java
+++ /dev/null
@@ -1,295 +0,0 @@
-/*
- * Initially from https://github.com/FRC3620/FRC3620_2020_GalacticSenate
- */
-
-package frc.lib.team3061.util;
-
-import edu.wpi.first.hal.can.CANJNI;
-import frc.lib.team3061.util.CANDeviceId.CANDeviceType;
-import frc.lib.team6328.util.Alert;
-import frc.lib.team6328.util.Alert.AlertType;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-import java.util.*;
-
-public class CANDeviceFinder {
- Set In the future, this interface may be further abstracted to not be coupled to PhotonVision.
+ * Currently, the abstraction is used to simulate vision.
+ */
public interface VisionIO {
public static class VisionIOInputs implements LoggableInputs {
PhotonPipelineResult lastResult = new PhotonPipelineResult(0, new ArrayList<>());
double lastTimestamp = 0.0;
- boolean hasNewResult = false;
public void toLog(LogTable table) {
+ // since AdvantageKit doesn't support the PhotonPipelineResult type, log it as a byte array
byte[] photonPacketBytes = new byte[lastResult.getPacketSize()];
lastResult.populatePacket(new Packet(photonPacketBytes));
table.put("photonPacketBytes", photonPacketBytes);
table.put("lastTimestamp", lastTimestamp);
- table.put("hasNewResult", hasNewResult);
// log targets in a human-readable way
List All translations and poses are stored with the origin at the rightmost point on the BLUE
+ * ALLIANCE wall. Use the {@link #allianceFlip(Translation2d)} and {@link #allianceFlip(Pose2d)}
+ * methods to flip these values based on the current alliance color.
+ */
+public final class FieldConstants {
+ public static final double fieldLength = Units.inchesToMeters(651.25);
+ public static final double fieldWidth = Units.inchesToMeters(315.5);
+ public static final double tapeWidth = Units.inchesToMeters(2.0);
+
+ // Dimensions for community and charging station, including the tape.
+ public static final class Community {
+ // Region dimensions
+ public static final double innerX = 0.0;
+ public static final double midX =
+ Units.inchesToMeters(132.375); // Tape to the left of charging station
+ public static final double outerX =
+ Units.inchesToMeters(193.25); // Tape to the right of charging station
+ public static final double leftY = Units.feetToMeters(18.0);
+ public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth;
+ public static final double rightY = 0.0;
+ public static final Translation2d[] regionCorners =
+ new Translation2d[] {
+ new Translation2d(innerX, rightY),
+ new Translation2d(innerX, leftY),
+ new Translation2d(midX, leftY),
+ new Translation2d(midX, midY),
+ new Translation2d(outerX, midY),
+ new Translation2d(outerX, rightY),
+ };
+
+ // Charging station dimensions
+ public static final double chargingStationLength = Units.inchesToMeters(76.125);
+ public static final double chargingStationWidth = Units.inchesToMeters(97.25);
+ public static final double chargingStationOuterX = outerX - tapeWidth;
+ public static final double chargingStationInnerX =
+ chargingStationOuterX - chargingStationLength;
+ public static final double chargingStationLeftY = midY - tapeWidth;
+ public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth;
+ public static final Translation2d[] chargingStationCorners =
+ new Translation2d[] {
+ new Translation2d(chargingStationInnerX, chargingStationRightY),
+ new Translation2d(chargingStationInnerX, chargingStationLeftY),
+ new Translation2d(chargingStationOuterX, chargingStationRightY),
+ new Translation2d(chargingStationOuterX, chargingStationLeftY)
+ };
+
+ // Cable bump
+ public static final double cableBumpInnerX =
+ innerX + Grids.outerX + Units.inchesToMeters(95.25);
+ public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7);
+ public static final Translation2d[] cableBumpCorners =
+ new Translation2d[] {
+ new Translation2d(cableBumpInnerX, 0.0),
+ new Translation2d(cableBumpInnerX, chargingStationRightY),
+ new Translation2d(cableBumpOuterX, 0.0),
+ new Translation2d(cableBumpOuterX, chargingStationRightY)
+ };
+ }
+
+ // Dimensions for grids and nodes
+ public static final class Grids {
+ // X layout
+ public static final double outerX = Units.inchesToMeters(54.25);
+ public static final double lowX =
+ outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered when under cube nodes
+ public static final double midX = outerX - Units.inchesToMeters(22.75);
+ public static final double highX = outerX - Units.inchesToMeters(39.75);
+
+ // Y layout
+ public static final int nodeRowCount = 9;
+ public static final double nodeFirstY = Units.inchesToMeters(20.19);
+ public static final double nodeSeparationY = Units.inchesToMeters(22.0);
+
+ // Z layout
+ public static final double cubeEdgeHigh = Units.inchesToMeters(3.0);
+ public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh;
+ public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh;
+ public static final double highConeZ = Units.inchesToMeters(46.0);
+ public static final double midConeZ = Units.inchesToMeters(34.0);
+
+ // Translations (all nodes in the same column/row have the same X/Y coordinate)
+ public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount];
+ public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount];
+ public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount];
+ public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount];
+ public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount];
+
+ static {
+ for (int i = 0; i < nodeRowCount; i++) {
+ boolean isCube = i == 1 || i == 4 || i == 7;
+ lowTranslations[i] = new Translation2d(lowX, nodeFirstY + nodeSeparationY * i);
+ midTranslations[i] = new Translation2d(midX, nodeFirstY + nodeSeparationY * i);
+ mid3dTranslations[i] =
+ new Translation3d(midX, nodeFirstY + nodeSeparationY * i, isCube ? midCubeZ : midConeZ);
+ high3dTranslations[i] =
+ new Translation3d(
+ highX, nodeFirstY + nodeSeparationY * i, isCube ? highCubeZ : highConeZ);
+ highTranslations[i] = new Translation2d(highX, nodeFirstY + nodeSeparationY * i);
+ }
+ }
+
+ // Complex low layout (shifted to account for cube vs cone rows and wide edge nodes)
+ public static final double complexLowXCones =
+ outerX - Units.inchesToMeters(16.0) / 2.0; // Centered X under cone nodes
+ public static final double complexLowXCubes = lowX; // Centered X under cube nodes
+ public static final double complexLowOuterYOffset =
+ nodeFirstY - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0);
+
+ public static final Translation2d[] complexLowTranslations =
+ new Translation2d[] {
+ new Translation2d(complexLowXCones, nodeFirstY - complexLowOuterYOffset),
+ new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 1),
+ new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 2),
+ new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 3),
+ new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 4),
+ new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 5),
+ new Translation2d(complexLowXCones, nodeFirstY + nodeSeparationY * 6),
+ new Translation2d(complexLowXCubes, nodeFirstY + nodeSeparationY * 7),
+ new Translation2d(
+ complexLowXCones, nodeFirstY + nodeSeparationY * 8 + complexLowOuterYOffset),
+ };
+ }
+
+ // Dimensions for loading zone and substations, including the tape
+ public static final class LoadingZone {
+ // Region dimensions
+ public static final double width = Units.inchesToMeters(99.0);
+ public static final double innerX = FieldConstants.fieldLength;
+ public static final double midX = fieldLength - Units.inchesToMeters(132.25);
+ public static final double outerX = fieldLength - Units.inchesToMeters(264.25);
+ public static final double leftY = FieldConstants.fieldWidth;
+ public static final double midY = leftY - Units.inchesToMeters(50.5);
+ public static final double rightY = leftY - width;
+ public static final Translation2d[] regionCorners =
+ new Translation2d[] {
+ new Translation2d(
+ midX, rightY), // Start at lower left next to border with opponent community
+ new Translation2d(midX, midY),
+ new Translation2d(outerX, midY),
+ new Translation2d(outerX, leftY),
+ new Translation2d(innerX, leftY),
+ new Translation2d(innerX, rightY),
+ };
+
+ // Double substation dimensions
+ public static final double doubleSubstationLength = Units.inchesToMeters(14.0);
+ public static final double doubleSubstationWidth = Units.inchesToMeters(96.0);
+ public static final double doubleSubstationX = innerX - doubleSubstationLength;
+ public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375);
+
+ // Single substation dimensions
+ public static final double singleSubstationWidth = Units.inchesToMeters(22.75);
+ public static final double singleSubstationLeftX =
+ FieldConstants.fieldLength - doubleSubstationLength - Units.inchesToMeters(88.77);
+ public static final double singleSubstationCenterX =
+ singleSubstationLeftX + (singleSubstationWidth / 2.0);
+ public static final double singleSubstationRightX =
+ singleSubstationLeftX + singleSubstationWidth;
+ public static final Translation2d singleSubstationTranslation =
+ new Translation2d(singleSubstationCenterX, leftY);
+
+ public static final double singleSubstationHeight = Units.inchesToMeters(18.0);
+ public static final double singleSubstationLowZ = Units.inchesToMeters(27.125);
+ public static final double singleSubstationCenterZ =
+ singleSubstationLowZ + (singleSubstationHeight / 2.0);
+ public static final double singleSubstationHighZ =
+ singleSubstationLowZ + singleSubstationHeight;
+ }
+
+ // Locations of staged game pieces
+ public static final class StagingLocations {
+ public static final double centerOffsetX = Units.inchesToMeters(47.36);
+ public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36);
+ public static final double firstY = Units.inchesToMeters(36.19);
+ public static final double separationY = Units.inchesToMeters(48.0);
+ public static final Translation2d[] translations = new Translation2d[4];
+
+ static {
+ for (int i = 0; i < translations.length; i++) {
+ translations[i] = new Translation2d(positionX, firstY + (i * separationY));
+ }
+ }
+ }
+
+ // AprilTag locations (do not flip for red alliance)
+ public static final Map Subsystem-specific constants should be defined in the subsystem's own constant class.
+ * Constants that vary from robot to robot should be defined in the config classes.
+ *
* It is advised to statically import this class (or one of its inner classes) wherever the
* constants are needed, to reduce verbosity.
*/
public final class Constants {
+ // set to true in order to change all Tunable values via Shuffleboard
public static final boolean TUNING_MODE = false;
private static final RobotType ROBOT = RobotType.ROBOT_SIMBOT;
@@ -31,7 +35,7 @@ public final class Constants {
// FIXME: update for various robots
public enum RobotType {
- ROBOT_2022_SIERRA,
+ ROBOT_2023_NOVA,
ROBOT_2023_MK4I,
ROBOT_DEFAULT,
ROBOT_SIMBOT
@@ -55,7 +59,7 @@ public static RobotType getRobot() {
public static Mode getMode() {
switch (getRobot()) {
case ROBOT_DEFAULT:
- case ROBOT_2022_SIERRA:
+ case ROBOT_2023_NOVA:
case ROBOT_2023_MK4I:
return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY;
diff --git a/src/main/java/frc/robot/Field2d.java b/src/main/java/frc/robot/Field2d.java
new file mode 100644
index 00000000..b5a10eb1
--- /dev/null
+++ b/src/main/java/frc/robot/Field2d.java
@@ -0,0 +1,246 @@
+package frc.robot;
+
+import com.pathplanner.lib.PathConstraints;
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPoint;
+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.wpilibj.DriverStation;
+import frc.lib.team3061.RobotConfig;
+import frc.lib.team6328.util.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.HashSet;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.Queue;
+import java.util.Set;
+
+/**
+ * This singleton class models the field as a collection of regions. This class is used to create a
+ * path from a starting pose in one region to an ending pose in another region that passes through
+ * the transition points defined for those regions.
+ *
+ * The coordinate system of the field is oriented such that the origin is in the lower left
+ * corner when the blue alliance is to the left (i.e., to the blue alliance driver's right). It can
+ * map poses to reflect the current alliance.
+ */
+public class Field2d {
+ private static Field2d instance = null;
+
+ private Region2d[] regions;
+ private DriverStation.Alliance alliance = DriverStation.Alliance.Invalid;
+
+ /**
+ * Get the singleton instance of the Field2d class.
+ *
+ * @return the singleton instance of the Field2d class
+ */
+ public static Field2d getInstance() {
+ if (instance == null) {
+ instance = new Field2d();
+ }
+ return instance;
+ }
+ /**
+ * Construct a Field2d from an array of regions. These regions should not be overlapping (aside
+ * from edges) and any regions with overlapping edges should be neighbors (see
+ * Region2d::addNeighbor).
+ *
+ * @param regions the regions that define the field
+ */
+ public void setRegions(Region2d[] regions) {
+ this.regions = regions;
+ }
+
+ /**
+ * Set the current alliance. This will adjust the coordinate system of the field and its regions.
+ *
+ * @param newAlliance the new alliance
+ */
+ public void updateAlliance(DriverStation.Alliance newAlliance) {
+ this.alliance = newAlliance;
+ for (Region2d region : regions) {
+ region.updateAlliance(newAlliance);
+ }
+ }
+
+ /**
+ * Translates the specified pose based on the current alliance.
+ *
+ * @param pose the pose to translate based on the current alliance
+ * @return the translated pose based on the current alliance
+ */
+ public Pose2d mapPoseToCurrentAlliance(Pose2d pose) {
+ if (this.alliance == DriverStation.Alliance.Red) {
+ return new Pose2d(
+ new Translation2d(
+ pose.getTranslation().getX(),
+ FieldConstants.fieldWidth - pose.getTranslation().getY()),
+ new Rotation2d(pose.getRotation().getCos(), -pose.getRotation().getSin()));
+ } else {
+ return pose;
+ }
+ }
+
+ /**
+ * Create a path from a starting pose in one region to an ending pose in another region that
+ * passes through the transition points defined for those regions.
+ *
+ * @param start the starting pose
+ * @param end the ending pose
+ * @param pathConstants the path constraints (i.e., max velocity, max acceleration)
+ * @param subsystem the drivetrain subsystem
+ * @return the path from the starting pose to the ending pose; null if no path exists
+ */
+ public PathPlannerTrajectory makePath(
+ Pose2d start, Pose2d end, PathConstraints pathConstants, Drivetrain subsystem) {
+ Region2d startRegion = null;
+ Region2d endRegion = null;
+
+ // find the starting and ending regions
+ for (Region2d region : regions) {
+ if (region.contains(start)) {
+ startRegion = region;
+ }
+ if (region.contains(end)) {
+ endRegion = region;
+ }
+ }
+
+ // make sure both start and end are on the field
+ if (startRegion == null || endRegion == null) return null;
+
+ // BFS to find the shortest path to the end
+ List This runs after the mode specific periodic methods, but before LiveWindow and SmartDashboard
* integrated updating.
@@ -132,9 +132,14 @@ public void robotPeriodic() {
logReceiverQueueAlert.set(Logger.getInstance().getReceiverQueueFault());
}
+ /** This method is invoked periodically when the robot is in the disabled state. */
@Override
public void disabledPeriodic() {
+ // check if the operator interface (e.g., joysticks) has changed
robotContainer.updateOI();
+
+ // check if the alliance color has changed based on the FMS data
+ robotContainer.checkAllianceColor();
}
/**
@@ -143,6 +148,11 @@ public void disabledPeriodic() {
*/
@Override
public void autonomousInit() {
+ // check if the alliance color has changed based on the FMS data; the current alliance color is
+ // not guaranteed to be correct until the start of autonomous
+ robotContainer.checkAllianceColor();
+
+ robotContainer.autonomousInit();
autonomousCommand = robotContainer.getAutonomousCommand();
// schedule the autonomous command
@@ -151,7 +161,7 @@ public void autonomousInit() {
}
}
- /** This method is invoked at the start of the teleoperated period. */
+ /** This method is invoked at the start of the teleop period. */
@Override
public void teleopInit() {
/*
@@ -162,6 +172,13 @@ public void teleopInit() {
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
+
+ // check if the alliance color has changed based on the FMS data; if the robot power cycled
+ // during a match, this would be the first opportunity to check the alliance color based on FMS
+ // data.
+ robotContainer.checkAllianceColor();
+
+ robotContainer.teleopInit();
}
/** This method is invoked at the start of the test period. */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 28f27f16..b9620c7e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,14 +4,19 @@
package frc.robot;
+import static frc.robot.Constants.*;
+import static frc.robot.FieldRegionConstants.*;
+
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.FollowPathWithEvents;
+import com.pathplanner.lib.server.PathPlannerServer;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.lib.team3061.RobotConfig;
@@ -33,13 +38,17 @@
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.commands.FeedForwardCharacterization.FeedForwardCharacterizationData;
import frc.robot.commands.FollowPath;
+import frc.robot.commands.RotateToAngle;
import frc.robot.commands.TeleopSwerve;
import frc.robot.configs.DefaultRobotConfig;
import frc.robot.configs.MK4IRobotConfig;
-import frc.robot.configs.SierraRobotConfig;
+import frc.robot.configs.NovaRobotConfig;
import frc.robot.operator_interface.OISelector;
import frc.robot.operator_interface.OperatorInterface;
import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.subsystem.Subsystem;
+import frc.robot.subsystems.subsystem.SubsystemIO;
+import frc.robot.subsystems.subsystem.SubsystemIOTalonFX;
import java.io.IOException;
import java.util.ArrayList;
import java.util.HashMap;
@@ -57,7 +66,9 @@ public class RobotContainer {
private OperatorInterface oi = new OperatorInterface() {};
private RobotConfig config;
private Drivetrain drivetrain;
+ private Alliance lastAlliance = DriverStation.Alliance.Invalid;
private Vision vision;
+ private Subsystem subsystem;
// use AdvantageKit's LoggedDashboardChooser instead of SendableChooser to ensure accurate logging
private final LoggedDashboardChooser Requires: the Drivetrain subsystem
+ *
+ * Finished When: the robot is at the specified pose (within the specified tolerances)
+ *
+ * At End: stops the drivetrain
+ */
+public class DriveToPose extends CommandBase {
+ private final Drivetrain drivetrain;
+ private final Supplier For following a predetermined trajectory, refer to the FollowPath Command class. For driving
+ * in a straight line to a pose, refer to the DriveToPose Command class.
+ *
+ * Requires: the Drivetrain subsystem
+ *
+ * Finished When: trajectory is null or time of specified trajectory has elapsed (from
+ * PPSwerveControllerCommand object)
+ *
+ * At End: stops the drivetrain, sets trajectory to null
+ */
+public class MoveToPose extends CommandBase {
+ private Supplier This method can be overridden to translate the end pose for a specific scenario (e.g.,
+ * account for the robot width).
+ */
+ protected Pose2d endPose() {
+ Pose2d endPose = endPoseSupplier.get();
+
+ Pose2d translatedEndPose = Field2d.getInstance().mapPoseToCurrentAlliance(endPose);
+
+ Logger.getInstance().recordOutput("MoveToPose/endPose", translatedEndPose);
+
+ return translatedEndPose;
+ }
+}
diff --git a/src/main/java/frc/robot/commands/RotateToAngle.java b/src/main/java/frc/robot/commands/RotateToAngle.java
new file mode 100644
index 00000000..9925e7e3
--- /dev/null
+++ b/src/main/java/frc/robot/commands/RotateToAngle.java
@@ -0,0 +1,172 @@
+package frc.robot.commands;
+
+import static frc.robot.Constants.*;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.lib.team3061.RobotConfig;
+import frc.lib.team6328.util.TunableNumber;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * This command, when executed, instructs the drivetrain subsystem to rotate to the specified angle
+ * while driving based on the supplied x and y values (e.g., from a joystick). The execute method
+ * invokes the drivetrain subsystem's drive method.
+ *
+ * Requires: the Drivetrain subsystem
+ *
+ * Finished When: the robot is at the specified angle (within the specified tolerances)
+ *
+ * At End: nothing (the drivetrain is left in whatever state it was in when the command finished)
+ */
+public class RotateToAngle extends CommandBase {
+ private final Drivetrain drivetrain;
+ private final DoubleSupplier targetAngleSupplier;
+ private final DoubleSupplier translationXSupplier;
+ private final DoubleSupplier translationYSupplier;
+
+ protected static final TunableNumber thetaKp = new TunableNumber("RotateToAngle/ThetaKp", 2);
+ protected static final TunableNumber thetaKi = new TunableNumber("RotateToAngle/ThetaKi", 10.0);
+ protected static final TunableNumber thetaKd = new TunableNumber("RotateToAngle/ThetaKd", 0.1);
+ protected static final TunableNumber thetaMaxVelocity =
+ new TunableNumber("RotateToAngle/ThetaMaxVelocity", 8);
+ protected static final TunableNumber thetaMaxAcceleration =
+ new TunableNumber("RotateToAngle/ThetaMaxAcceleration", 100);
+ protected static final TunableNumber thetaTolerance =
+ new TunableNumber("RotateToAngle/ThetaTolerance", 0.008);
+
+ protected final ProfiledPIDController thetaController =
+ new ProfiledPIDController(
+ thetaKp.get(),
+ thetaKi.get(),
+ thetaKd.get(),
+ new TrapezoidProfile.Constraints(thetaMaxVelocity.get(), thetaMaxAcceleration.get()),
+ LOOP_PERIOD_SECS);
+
+ /**
+ * Constructs a new RotateToAngle command that, when executed, instructs the drivetrain subsystem
+ * to rotate to the specified angle in place.
+ *
+ * @param drivetrain the drivetrain subsystem required by this command
+ * @param targetAngleSupplier the supplier of the target angle, in degrees. Zero degrees is away
+ * from the driver and increases in the CCW direction.
+ */
+ public RotateToAngle(Drivetrain drivetrain, DoubleSupplier targetAngleSupplier) {
+ this(drivetrain, () -> 0, () -> 0, targetAngleSupplier);
+ }
+
+ /**
+ * Constructs a new RotateToAngle command that, when executed, instructs the drivetrain subsystem
+ * to rotate to the specified angle while driving based on the supplied x and y values (e.g., from
+ * a joystick).
+ *
+ * @param drivetrain the drivetrain subsystem required by this command
+ * @param translationXSupplier the supplier of the x value as a percentage of the maximum velocity
+ * in the x direction as defined by the standard field or robot coordinate system
+ * @param translationYSupplier the supplier of the y value as a percentage of the maximum velocity
+ * in the y direction as defined by the standard field or robot coordinate system
+ * @param targetAngleSupplier the supplier of the target angle, in degrees. Zero degrees is away
+ * from the driver and increases in the CCW direction.
+ */
+ public RotateToAngle(
+ Drivetrain drivetrain,
+ DoubleSupplier translationXSupplier,
+ DoubleSupplier translationYSupplier,
+ DoubleSupplier targetAngleSupplier) {
+ this.drivetrain = drivetrain;
+ addRequirements(drivetrain);
+ this.translationXSupplier = translationXSupplier;
+ this.translationYSupplier = translationYSupplier;
+ this.targetAngleSupplier = targetAngleSupplier;
+ }
+
+ /**
+ * This method is invoked once when this command is scheduled. It resets all the PID controller
+ * and queries the target angle. It is critical that this initialization occurs in this method and
+ * not the constructor as this object is constructed well before the command is scheduled and the
+ * robot's pose will definitely have changed and the target angle may not be known until this
+ * command is scheduled.
+ */
+ @Override
+ public void initialize() {
+ Logger.getInstance().recordOutput("ActiveCommands/RotateToAngle", true);
+
+ Pose2d currentPose = drivetrain.getPose();
+ thetaController.reset(currentPose.getRotation().getRadians());
+ thetaController.setTolerance(thetaTolerance.get());
+
+ // configure the controller such that the range of values is centered on the target angle
+ thetaController.enableContinuousInput(
+ Units.degreesToRadians(this.targetAngleSupplier.getAsDouble()) - Math.PI,
+ Units.degreesToRadians(this.targetAngleSupplier.getAsDouble()) + Math.PI);
+
+ Logger.getInstance().recordOutput("RotateToAngle/AngleDeg", targetAngleSupplier.getAsDouble());
+ }
+
+ /**
+ * This method is invoked periodically while this command is scheduled. It calculates the
+ * velocities based on the current and target rotation and invokes the drivetrain subsystem's
+ * drive method.
+ */
+ @Override
+ public void execute() {
+ // update from tunable numbers
+ if (thetaKp.hasChanged()
+ || thetaKd.hasChanged()
+ || thetaKi.hasChanged()
+ || thetaMaxVelocity.hasChanged()
+ || thetaMaxAcceleration.hasChanged()
+ || thetaTolerance.hasChanged()) {
+ thetaController.setP(thetaKp.get());
+ thetaController.setI(thetaKi.get());
+ thetaController.setD(thetaKd.get());
+ thetaController.setConstraints(
+ new TrapezoidProfile.Constraints(thetaMaxVelocity.get(), thetaMaxAcceleration.get()));
+ thetaController.setTolerance(thetaTolerance.get());
+ }
+
+ Pose2d currentPose = drivetrain.getPose();
+ double thetaVelocity =
+ thetaController.calculate(
+ currentPose.getRotation().getRadians(),
+ Units.degreesToRadians(this.targetAngleSupplier.getAsDouble()));
+ if (thetaController.atGoal()) {
+ thetaVelocity = 0.0;
+ }
+ double xPercentage = TeleopSwerve.modifyAxis(translationXSupplier.getAsDouble(), 2.0);
+ double yPercentage = TeleopSwerve.modifyAxis(translationYSupplier.getAsDouble(), 2.0);
+
+ double xVelocity = xPercentage * RobotConfig.getInstance().getRobotMaxVelocity();
+ double yVelocity = yPercentage * RobotConfig.getInstance().getRobotMaxVelocity();
+
+ drivetrain.drive(xVelocity, yVelocity, thetaVelocity, true, false);
+ }
+
+ /**
+ * This method returns true if the command has finished. It is invoked periodically while this
+ * command is scheduled (after execute is invoked). This command is considered finished if the
+ * rotational controller is at its goal.
+ *
+ * @return true if the command has finished
+ */
+ @Override
+ public boolean isFinished() {
+ return thetaController.atGoal();
+ }
+
+ /**
+ * This method will be invoked when this command finishes or is interrupted. The drivetrain is
+ * left in whatever state it was in when the command finished.
+ *
+ * @param interrupted true if the command was interrupted by another command being scheduled
+ */
+ @Override
+ public void end(boolean interrupted) {
+ Logger.getInstance().recordOutput("ActiveCommands/RotateToAngle", false);
+ }
+}
diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java
index d164e7c6..f5941d7e 100644
--- a/src/main/java/frc/robot/commands/TeleopSwerve.java
+++ b/src/main/java/frc/robot/commands/TeleopSwerve.java
@@ -2,13 +2,17 @@
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.lib.team3061.RobotConfig;
+import frc.lib.team6328.util.TunableNumber;
+import frc.robot.Constants;
import frc.robot.subsystems.drivetrain.Drivetrain;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
/**
* This command, when executed, instructs the drivetrain subsystem to drive based on the specified
- * values from the controller(s). This command is designed to be the default command for the
+ * values from the controller(s) and enabled features. Configurable deadband and power function are
+ * applied to the controller inputs. If the robot isn't in "turbo" mode, the acceleration is limited
+ * based on the configurable constraints. This command is designed to be the default command for the
* drivetrain subsystem.
*
* Requires: the Drivetrain subsystem
@@ -24,12 +28,25 @@ public class TeleopSwerve extends CommandBase {
private final DoubleSupplier translationYSupplier;
private final DoubleSupplier rotationSupplier;
- public static final double DEADBAND = 0.1;
+ private static final double DEADBAND = 0.1;
+ private double lastAngularVelocity;
+ private double lastXVelocity;
+ private double lastYVelocity;
private final double maxVelocityMetersPerSecond = RobotConfig.getInstance().getRobotMaxVelocity();
private final double maxAngularVelocityRadiansPerSecond =
RobotConfig.getInstance().getRobotMaxAngularVelocity();
+ private final TunableNumber maxTurnAcceleration =
+ new TunableNumber(
+ "TeleopSwerve/maxTurnAcceleration",
+ RobotConfig.getInstance().getRobotMaxTurnAcceleration());
+ private final TunableNumber joystickPower = new TunableNumber("TeleopSwerve/joystickPower", 2.0);
+ private final TunableNumber maxDriveAcceleration =
+ new TunableNumber(
+ "TeleopSwerve/maxDriveAcceleration",
+ RobotConfig.getInstance().getRobotMaxDriveAcceleration());
+
/**
* Create a new TeleopSwerve command object.
*
@@ -54,33 +71,76 @@ public TeleopSwerve(
addRequirements(drivetrain);
}
+ @Override
+ public void initialize() {
+ Logger.getInstance().recordOutput("ActiveCommands/TeleopSwerve", true);
+ }
+
+ /**
+ * This method is invoked periodically while this command is scheduled. It calculates the
+ * velocities based on the supplied values and enabled features and invokes the drivetrain
+ * subsystem's drive method.
+ */
@Override
public void execute() {
- // invert the controller input and apply the deadband and squaring to make the robot more
- // responsive to small changes in the controller
- double xPercentage = modifyAxis(translationXSupplier.getAsDouble());
- double yPercentage = modifyAxis(translationYSupplier.getAsDouble());
- double rotationPercentage = modifyAxis(rotationSupplier.getAsDouble());
+ // invert the controller input and apply the deadband and exponential function to make the robot
+ // more responsive to small changes in the controller
+ double xPercentage = modifyAxis(translationXSupplier.getAsDouble(), joystickPower.get());
+ double yPercentage = modifyAxis(translationYSupplier.getAsDouble(), joystickPower.get());
+ double rotationPercentage = modifyAxis(rotationSupplier.getAsDouble(), joystickPower.get());
double xVelocity = xPercentage * maxVelocityMetersPerSecond;
double yVelocity = yPercentage * maxVelocityMetersPerSecond;
double rotationalVelocity = rotationPercentage * maxAngularVelocityRadiansPerSecond;
- Logger.getInstance().recordOutput("ActiveCommands/TeleopSwerve", true);
Logger.getInstance().recordOutput("TeleopSwerve/xVelocity", xVelocity);
Logger.getInstance().recordOutput("TeleopSwerve/yVelocity", yVelocity);
Logger.getInstance().recordOutput("TeleopSwerve/rotationalVelocity", rotationalVelocity);
- drivetrain.drive(xVelocity, yVelocity, rotationalVelocity, true);
+ // if the robot is not in turbo mode, limit the acceleration
+ if (!drivetrain.getTurbo()) {
+ double driveAccelerationMetersPer20Ms =
+ maxDriveAcceleration.get() * Constants.LOOP_PERIOD_SECS;
+
+ if (Math.abs(xVelocity - lastXVelocity) > driveAccelerationMetersPer20Ms) {
+ xVelocity =
+ lastXVelocity
+ + Math.copySign(driveAccelerationMetersPer20Ms, xVelocity - lastXVelocity);
+ }
+
+ if (Math.abs(yVelocity - lastYVelocity) > driveAccelerationMetersPer20Ms) {
+ yVelocity =
+ lastYVelocity
+ + Math.copySign(driveAccelerationMetersPer20Ms, yVelocity - lastYVelocity);
+ }
+
+ double turnAccelerationRadiansPer20Ms =
+ maxTurnAcceleration.get() * Constants.LOOP_PERIOD_SECS;
+
+ if (Math.abs(rotationalVelocity - lastAngularVelocity) > turnAccelerationRadiansPer20Ms) {
+ rotationalVelocity =
+ lastAngularVelocity
+ + Math.copySign(
+ turnAccelerationRadiansPer20Ms, rotationalVelocity - lastAngularVelocity);
+ }
+ }
+
+ lastXVelocity = xVelocity;
+ lastYVelocity = yVelocity;
+ lastAngularVelocity = rotationalVelocity;
+
+ drivetrain.drive(xVelocity, yVelocity, rotationalVelocity, true, false);
}
+ /**
+ * This method will be invoked when this command finishes or is interrupted. The drivetrain is
+ * left in whatever state it was in when the command finished.
+ *
+ * @param interrupted true if the command was interrupted by another command being scheduled
+ */
@Override
public void end(boolean interrupted) {
- this.drivetrain.stop();
-
- super.end(interrupted);
-
Logger.getInstance().recordOutput("ActiveCommands/TeleopSwerve", false);
}
@@ -88,17 +148,14 @@ public void end(boolean interrupted) {
* Squares the specified value, while preserving the sign. This method is used on all joystick
* inputs. This is useful as a non-linear range is more natural for the driver.
*
- * @param value
- * @return
+ * @param value the raw controller value
+ * @param power the power to which the value should be raised
+ * @return the modified value
*/
- private static double modifyAxis(double value) {
- // Deadband
- value = deadband(value, DEADBAND);
-
- // Square the axis
- value = Math.copySign(value * value, value);
-
- return value;
+ public static double modifyAxis(double value, double power) {
+ double modifiedValue = deadband(value, DEADBAND);
+ modifiedValue = Math.copySign(Math.pow(modifiedValue, power), modifiedValue);
+ return modifiedValue;
}
private static double deadband(double value, double deadband) {
diff --git a/src/main/java/frc/robot/configs/DefaultRobotConfig.java b/src/main/java/frc/robot/configs/DefaultRobotConfig.java
index 0dc03087..e85146e1 100644
--- a/src/main/java/frc/robot/configs/DefaultRobotConfig.java
+++ b/src/main/java/frc/robot/configs/DefaultRobotConfig.java
@@ -72,7 +72,7 @@ public class DefaultRobotConfig extends RobotConfig {
private static final String CAN_BUS_NAME = "";
// FIXME: specify the name of the camera used for detecting AprilTags
- private static final String CAMERA_NAME = "ov9268";
+ private static final String CAMERA_NAME = "OV9281";
// FIXME: update this with the actual transform from the robot to the camera
private static final Transform3d ROBOT_TO_CAMERA =
@@ -221,8 +221,8 @@ public double getRobotLengthWithBumpers() {
}
@Override
- public Transform3d getRobotToCameraTransform() {
- return ROBOT_TO_CAMERA;
+ public Transform3d[] getRobotToCameraTransforms() {
+ return new Transform3d[] {ROBOT_TO_CAMERA};
}
@Override
@@ -281,8 +281,8 @@ public String getCANBusName() {
}
@Override
- public String getCameraName() {
- return CAMERA_NAME;
+ public String[] getCameraNames() {
+ return new String[] {CAMERA_NAME};
}
@Override
diff --git a/src/main/java/frc/robot/configs/MK4IRobotConfig.java b/src/main/java/frc/robot/configs/MK4IRobotConfig.java
index 76d23b28..48eab3f7 100644
--- a/src/main/java/frc/robot/configs/MK4IRobotConfig.java
+++ b/src/main/java/frc/robot/configs/MK4IRobotConfig.java
@@ -12,39 +12,35 @@
*/
public class MK4IRobotConfig extends RobotConfig {
- // FIXME: update all CAN IDs and steer offsets
- private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 7;
- private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 6;
- private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 8;
- private static final double FRONT_LEFT_MODULE_STEER_OFFSET = 118.0371;
-
- private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 13;
- private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 12;
- private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 14;
- private static final double FRONT_RIGHT_MODULE_STEER_OFFSET = 102.9968;
-
- private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 10;
- private static final int BACK_LEFT_MODULE_STEER_MOTOR = 9;
- private static final int BACK_LEFT_MODULE_STEER_ENCODER = 11;
- private static final double BACK_LEFT_MODULE_STEER_OFFSET = -189.7051;
-
- private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 16;
- private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 15;
- private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 17;
- private static final double BACK_RIGHT_MODULE_STEER_OFFSET = 40.3335;
+ private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 13;
+ private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 12;
+ private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14;
+ private static final double FRONT_LEFT_MODULE_STEER_OFFSET = 37.35;
+
+ private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 16;
+ private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 15;
+ private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17;
+ private static final double FRONT_RIGHT_MODULE_STEER_OFFSET = 83.49;
+
+ private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7;
+ private static final int BACK_LEFT_MODULE_STEER_MOTOR = 6;
+ private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8;
+ private static final double BACK_LEFT_MODULE_STEER_OFFSET = 259.62;
+
+ private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10;
+ private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9;
+ private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11;
+ private static final double BACK_RIGHT_MODULE_STEER_OFFSET = 9.14;
private static final int GYRO_ID = 18;
- // FIXME: update robot dimensions
- private static final double TRACKWIDTH_METERS = 0.5715; // 22.5 inches
- private static final double WHEELBASE_METERS = 0.5969; // 23.5 inches
- private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.89; // meters
- private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.91; // meters
-
- // FIXME: tune PID values for the angle and drive motors for the swerve modules
+ private static final double TRACKWIDTH_METERS = 0.574675; // 22.625 inches
+ private static final double WHEELBASE_METERS = 0.619125; // 24.375 inches
+ private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.8255; // meters
+ private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.8255; // meters
/* Angle Motor PID Values */
- private static final double ANGLE_KP = 0.6;
+ private static final double ANGLE_KP = 0.4;
private static final double ANGLE_KI = 0.0;
private static final double ANGLE_KD = 12.0;
private static final double ANGLE_KF = 0.0;
@@ -52,48 +48,51 @@ public class MK4IRobotConfig extends RobotConfig {
/* Drive Motor PID Values */
private static final double DRIVE_KP = 0.10;
private static final double DRIVE_KI = 0.0;
- private static final double DRIVE_KD = 0.0;
+ private static final double DRIVE_KD = 5.5;
private static final double DRIVE_KF = 0.0;
- // FIXME: characterize the drivetrain and update these constants
- private static final double DRIVE_KS = 0.55493;
- private static final double DRIVE_KV = 2.3014;
+ private static final double DRIVE_KS = 0.25988;
+ private static final double DRIVE_KV = 2.46330;
private static final double DRIVE_KA = 0.12872;
- // FIXME: specify the type of swerve module (MK4 and MK4i are supported)
private static final SwerveType SWERVE_TYPE = SwerveType.MK4I;
- // FIXME: determine maximum velocities empirically
- private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.25;
+ private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.78;
private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05;
+ private static final double SLOW_MODE_MULTIPLIER = 0.75;
+
+ private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0;
+ private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2.0 * Math.PI;
- // FIXME: specify the name of the CANivore CAN FD bus as appropriate (an empty string uses the
- // default CAN bus)
- private static final String CAN_BUS_NAME = "";
+ private static final String CAN_BUS_NAME = "canbus1";
- // FIXME: specify the name of the camera used for detecting AprilTags
- private static final String CAMERA_NAME = "ov9268";
+ private static final String CAMERA_NAME = "OV9281";
- // FIXME: update this with the actual transform from the robot to the camera
private static final Transform3d ROBOT_TO_CAMERA =
- new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
+ new Transform3d(new Translation3d(0.356, 0.0, 0.465), new Rotation3d(0, 0, 0));
- // FIXME: specify the configuration for pneumatics
private static final int PNEUMATICS_HUB_ID = 20;
private static final int FLOW_SENSOR_CHANNEL = 0;
private static final int REV_HIGH_PRESSURE_SENSOR_CHANNEL = 0;
private static final int REV_LOW_PRESSURE_SENSOR_CHANNEL = 1;
- // FIXME: specify maximum velocity and acceleration and tune PID values for auto paths
-
private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0;
private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0;
private static final double AUTO_DRIVE_P_CONTROLLER = 6.0;
private static final double AUTO_DRIVE_I_CONTROLLER = 0.0;
private static final double AUTO_DRIVE_D_CONTROLLER = 0.0;
- private static final double AUTO_TURN_P_CONTROLLER = 10.0;
- private static final double AUTO_TURN_I_CONTROLLER = 0.0;
- private static final double AUTO_TURN_D_CONTROLLER = 0.0;
+ private static final double AUTO_TURN_P_CONTROLLER = 0.4;
+ private static final double AUTO_TURN_I_CONTROLLER = 1.6;
+ private static final double AUTO_TURN_D_CONTROLLER = 1.0;
+
+ // Drive to Pose constants
+ private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5;
+ private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0;
+ private static final double DRIVE_TO_POSE_THETA_KP = 18.0;
+ private static final double DRIVE_TO_POSE_THETA_KI = 10.0;
+ private static final double DRIVE_TO_POSE_THETA_KD = 0.0;
+ private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.08;
+ private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.008;
@Override
public double getSwerveAngleKP() {
@@ -221,8 +220,8 @@ public double getRobotLengthWithBumpers() {
}
@Override
- public Transform3d getRobotToCameraTransform() {
- return ROBOT_TO_CAMERA;
+ public Transform3d[] getRobotToCameraTransforms() {
+ return new Transform3d[] {ROBOT_TO_CAMERA};
}
@Override
@@ -230,6 +229,21 @@ public double getRobotMaxVelocity() {
return MAX_VELOCITY_METERS_PER_SECOND;
}
+ @Override
+ public double getRobotMaxDriveAcceleration() {
+ return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotMaxTurnAcceleration() {
+ return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotSlowModeMultiplier() {
+ return SLOW_MODE_MULTIPLIER;
+ }
+
@Override
public double getRobotMaxCoastVelocity() {
return MAX_COAST_VELOCITY_METERS_PER_SECOND;
@@ -281,8 +295,8 @@ public String getCANBusName() {
}
@Override
- public String getCameraName() {
- return CAMERA_NAME;
+ public String[] getCameraNames() {
+ return new String[] {CAMERA_NAME};
}
@Override
@@ -304,4 +318,61 @@ public int getRevHighPressureSensorChannel() {
public int getRevLowPressureSensorChannel() {
return REV_LOW_PRESSURE_SENSOR_CHANNEL;
}
+
+ @Override
+ public double getDriveToPoseDriveKP() {
+ return DRIVE_TO_POSE_DRIVE_KP;
+ }
+
+ @Override
+ public double getDriveToPoseDriveKD() {
+ return DRIVE_TO_POSE_DRIVE_KD;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKI() {
+ return DRIVE_TO_POSE_THETA_KI;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKP() {
+ return DRIVE_TO_POSE_THETA_KP;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKD() {
+ return DRIVE_TO_POSE_THETA_KD;
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxVelocity() {
+ return 0.5;
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxAcceleration() {
+ return getAutoMaxAcceleration();
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxVelocity() {
+ return getDriveToPoseDriveMaxVelocity()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxAcceleration() {
+ return getDriveToPoseDriveMaxAcceleration()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseDriveTolerance() {
+ return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS;
+ }
+
+ @Override
+ public double getDriveToPoseThetaTolerance() {
+ return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS;
+ }
}
diff --git a/src/main/java/frc/robot/configs/NovaRobotConfig.java b/src/main/java/frc/robot/configs/NovaRobotConfig.java
new file mode 100644
index 00000000..84a2f105
--- /dev/null
+++ b/src/main/java/frc/robot/configs/NovaRobotConfig.java
@@ -0,0 +1,381 @@
+package frc.robot.configs;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import frc.lib.team3061.RobotConfig;
+import frc.lib.team3061.swerve.SwerveModuleConstants.SwerveType;
+
+/*
+ * Refer to the README for how to represent your robot's configuration. For more information on
+ * these methods, refer to the documentation in the RobotConfig class.
+ */
+public class NovaRobotConfig extends RobotConfig {
+
+ private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 13;
+ private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 12;
+ private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14;
+ private static final double FRONT_LEFT_MODULE_STEER_OFFSET = 37.53;
+
+ private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 16;
+ private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 15;
+ private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17;
+ private static final double FRONT_RIGHT_MODULE_STEER_OFFSET = 100.02;
+
+ private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7;
+ private static final int BACK_LEFT_MODULE_STEER_MOTOR = 6;
+ private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8;
+ private static final double BACK_LEFT_MODULE_STEER_OFFSET = 268.51;
+
+ private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10;
+ private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9;
+ private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11;
+ private static final double BACK_RIGHT_MODULE_STEER_OFFSET = 8.88;
+
+ private static final int GYRO_ID = 3;
+
+ private static final double TRACKWIDTH_METERS = 0.523875; // 20.625
+ private static final double WHEELBASE_METERS = 0.52705; // 20.75
+ private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.8382; // meters //33 in
+ private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.8382; // meters // 33 in
+
+ /* Angle Motor PID Values */
+ private static final double ANGLE_KP = 0.4;
+ private static final double ANGLE_KI = 0.0;
+ private static final double ANGLE_KD = 12.0;
+ private static final double ANGLE_KF = 0.0;
+
+ /* Drive Motor PID Values */
+ private static final double DRIVE_KP = 0.10;
+ private static final double DRIVE_KI = 0.0;
+ private static final double DRIVE_KD = 5.5;
+ private static final double DRIVE_KF = 0.0;
+
+ private static final double DRIVE_KS = 0.28006;
+ private static final double DRIVE_KV = 2.32216;
+ private static final double DRIVE_KA = 0.12872;
+
+ private static final SwerveType SWERVE_TYPE = SwerveType.MK4I;
+
+ private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.78;
+ private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05;
+ private static final double SLOW_MODE_MULTIPLIER = 0.75;
+
+ private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 10.0;
+ private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 4.0 * Math.PI;
+
+ private static final String CAN_BUS_NAME = "canbus1";
+
+ private static final String CAMERA_NAME_0 = "OV9281";
+
+ private static final String CAMERA_NAME_1 = "OV9281R";
+
+ private static final Transform3d ROBOT_TO_CAMERA_0 =
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.406),
+ Units.inchesToMeters(6.603),
+ Units.inchesToMeters(49.240)),
+ new Rotation3d(0, Units.degreesToRadians(25), Units.degreesToRadians(30)));
+
+ private static final Transform3d ROBOT_TO_CAMERA_1 =
+ new Transform3d(
+ new Translation3d(
+ Units.inchesToMeters(-10.406),
+ Units.inchesToMeters(-6.603),
+ Units.inchesToMeters(49.240)),
+ new Rotation3d(0, Units.degreesToRadians(25), Units.degreesToRadians(-30)));
+
+ private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0;
+ private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0;
+ private static final double AUTO_DRIVE_P_CONTROLLER = 8.0;
+ private static final double AUTO_DRIVE_I_CONTROLLER = 0.0;
+ private static final double AUTO_DRIVE_D_CONTROLLER = 0.0;
+ private static final double AUTO_TURN_P_CONTROLLER = 0.2;
+ private static final double AUTO_TURN_I_CONTROLLER = 0.0;
+ private static final double AUTO_TURN_D_CONTROLLER = 0.0;
+
+ // Drive to Pose constants
+ private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5;
+ private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0;
+ private static final double DRIVE_TO_POSE_THETA_KP = 18.0;
+ private static final double DRIVE_TO_POSE_THETA_KI = 10.0;
+ private static final double DRIVE_TO_POSE_THETA_KD = 0.0;
+ private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.08;
+ private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.008;
+
+ private static final double SQUARING_SPEED_METERS_PER_SECOND = 1.0;
+
+ @Override
+ public double getSwerveAngleKP() {
+ return ANGLE_KP;
+ }
+
+ @Override
+ public double getSwerveAngleKI() {
+ return ANGLE_KI;
+ }
+
+ @Override
+ public double getSwerveAngleKD() {
+ return ANGLE_KD;
+ }
+
+ @Override
+ public double getSwerveAngleKF() {
+ return ANGLE_KF;
+ }
+
+ @Override
+ public double getSwerveDriveKP() {
+ return DRIVE_KP;
+ }
+
+ @Override
+ public double getSwerveDriveKI() {
+ return DRIVE_KI;
+ }
+
+ @Override
+ public double getSwerveDriveKD() {
+ return DRIVE_KD;
+ }
+
+ @Override
+ public double getSwerveDriveKF() {
+ return DRIVE_KF;
+ }
+
+ @Override
+ public double getDriveKS() {
+ return DRIVE_KS;
+ }
+
+ @Override
+ public double getDriveKV() {
+ return DRIVE_KV;
+ }
+
+ @Override
+ public double getDriveKA() {
+ return DRIVE_KA;
+ }
+
+ @Override
+ public SwerveType getSwerveType() {
+ return SWERVE_TYPE;
+ }
+
+ @Override
+ public int[] getSwerveDriveMotorCANIDs() {
+ return new int[] {
+ FRONT_LEFT_MODULE_DRIVE_MOTOR,
+ FRONT_RIGHT_MODULE_DRIVE_MOTOR,
+ BACK_LEFT_MODULE_DRIVE_MOTOR,
+ BACK_RIGHT_MODULE_DRIVE_MOTOR
+ };
+ }
+
+ @Override
+ public int[] getSwerveSteerMotorCANIDs() {
+ return new int[] {
+ FRONT_LEFT_MODULE_STEER_MOTOR,
+ FRONT_RIGHT_MODULE_STEER_MOTOR,
+ BACK_LEFT_MODULE_STEER_MOTOR,
+ BACK_RIGHT_MODULE_STEER_MOTOR
+ };
+ }
+
+ @Override
+ public int[] getSwerveSteerEncoderCANIDs() {
+ return new int[] {
+ FRONT_LEFT_MODULE_STEER_ENCODER,
+ FRONT_RIGHT_MODULE_STEER_ENCODER,
+ BACK_LEFT_MODULE_STEER_ENCODER,
+ BACK_RIGHT_MODULE_STEER_ENCODER
+ };
+ }
+
+ @Override
+ public double[] getSwerveSteerOffsets() {
+ return new double[] {
+ FRONT_LEFT_MODULE_STEER_OFFSET,
+ FRONT_RIGHT_MODULE_STEER_OFFSET,
+ BACK_LEFT_MODULE_STEER_OFFSET,
+ BACK_RIGHT_MODULE_STEER_OFFSET
+ };
+ }
+
+ @Override
+ public int getGyroCANID() {
+ return GYRO_ID;
+ }
+
+ @Override
+ public double getTrackwidth() {
+ return TRACKWIDTH_METERS;
+ }
+
+ @Override
+ public double getWheelbase() {
+ return WHEELBASE_METERS;
+ }
+
+ @Override
+ public double getRobotWidthWithBumpers() {
+ return ROBOT_WIDTH_WITH_BUMPERS;
+ }
+
+ @Override
+ public double getRobotLengthWithBumpers() {
+ return ROBOT_LENGTH_WITH_BUMPERS;
+ }
+
+ @Override
+ public Transform3d[] getRobotToCameraTransforms() {
+ return new Transform3d[] {ROBOT_TO_CAMERA_0, ROBOT_TO_CAMERA_1};
+ }
+
+ @Override
+ public double getRobotMaxVelocity() {
+ return MAX_VELOCITY_METERS_PER_SECOND;
+ }
+
+ @Override
+ public double getRobotMaxDriveAcceleration() {
+ return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotMaxTurnAcceleration() {
+ return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotSlowModeMultiplier() {
+ return SLOW_MODE_MULTIPLIER;
+ }
+
+ @Override
+ public double getRobotMaxCoastVelocity() {
+ return MAX_COAST_VELOCITY_METERS_PER_SECOND;
+ }
+
+ @Override
+ public double getAutoMaxSpeed() {
+ return AUTO_MAX_SPEED_METERS_PER_SECOND;
+ }
+
+ @Override
+ public double getAutoMaxAcceleration() {
+ return AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getAutoDriveKP() {
+ return AUTO_DRIVE_P_CONTROLLER;
+ }
+
+ @Override
+ public double getAutoDriveKI() {
+ return AUTO_DRIVE_I_CONTROLLER;
+ }
+
+ @Override
+ public double getAutoDriveKD() {
+ return AUTO_DRIVE_D_CONTROLLER;
+ }
+
+ @Override
+ public double getAutoTurnKP() {
+ return AUTO_TURN_P_CONTROLLER;
+ }
+
+ @Override
+ public double getAutoTurnKI() {
+ return AUTO_TURN_I_CONTROLLER;
+ }
+
+ @Override
+ public double getAutoTurnKD() {
+ return AUTO_TURN_D_CONTROLLER;
+ }
+
+ @Override
+ public String getCANBusName() {
+ return CAN_BUS_NAME;
+ }
+
+ @Override
+ public String[] getCameraNames() {
+ return new String[] {CAMERA_NAME_0, CAMERA_NAME_1};
+ }
+
+ @Override
+ public double getDriveToPoseDriveKP() {
+ return DRIVE_TO_POSE_DRIVE_KP;
+ }
+
+ @Override
+ public double getDriveToPoseDriveKD() {
+ return DRIVE_TO_POSE_DRIVE_KD;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKI() {
+ return DRIVE_TO_POSE_THETA_KI;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKP() {
+ return DRIVE_TO_POSE_THETA_KP;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKD() {
+ return DRIVE_TO_POSE_THETA_KD;
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxVelocity() {
+ return 0.5;
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxAcceleration() {
+ return getAutoMaxAcceleration();
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxVelocity() {
+ return getDriveToPoseDriveMaxVelocity()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxAcceleration() {
+ return getDriveToPoseDriveMaxAcceleration()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseDriveTolerance() {
+ return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS;
+ }
+
+ @Override
+ public double getDriveToPoseThetaTolerance() {
+ return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS;
+ }
+
+ @Override
+ public int getPneumaticsHubCANID() {
+ return 0;
+ }
+
+ @Override
+ public double getMoveToPathFinalVelocity() {
+ return SQUARING_SPEED_METERS_PER_SECOND;
+ }
+}
diff --git a/src/main/java/frc/robot/configs/SierraRobotConfig.java b/src/main/java/frc/robot/configs/SierraRobotConfig.java
index cd782ed0..6fc70dee 100644
--- a/src/main/java/frc/robot/configs/SierraRobotConfig.java
+++ b/src/main/java/frc/robot/configs/SierraRobotConfig.java
@@ -30,7 +30,7 @@ public class SierraRobotConfig extends RobotConfig {
private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 16;
private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 15;
private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 17;
- private static final double BACK_RIGHT_MODULE_STEER_OFFSET = 40.86;
+ private static final double BACK_RIGHT_MODULE_STEER_OFFSET = 40.78;
private static final int GYRO_ID = 18;
@@ -59,12 +59,15 @@ public class SierraRobotConfig extends RobotConfig {
private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.25;
private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05;
+ private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0;
+ private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 4.0;
+ private static final double SLOW_MODE_MULTIPLIER = 0.75;
private static final String CAN_BUS_NAME = "";
private static final String CAMERA_NAME = "OV9281";
- private static final Transform3d ROBOT_TO_CAMERA =
+ private static final Transform3d ROBOT_TO_CAMERA_0 =
new Transform3d(new Translation3d(0.254, 0, 0.648), new Rotation3d(0, 0, 0));
private static final int PNEUMATICS_HUB_ID = 20;
@@ -81,6 +84,14 @@ public class SierraRobotConfig extends RobotConfig {
private static final double AUTO_TURN_I_CONTROLLER = 0.0;
private static final double AUTO_TURN_D_CONTROLLER = 0.0;
+ // Drive to Pose constants
+ private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5;
+ private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0;
+ private static final double DRIVE_TO_POSE_THETA_KP = 7.0;
+ private static final double DRIVE_TO_POSE_THETA_KD = 0.0;
+ private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.01;
+ private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.035;
+
@Override
public double getSwerveAngleKP() {
return ANGLE_KP;
@@ -207,8 +218,8 @@ public double getRobotLengthWithBumpers() {
}
@Override
- public Transform3d getRobotToCameraTransform() {
- return ROBOT_TO_CAMERA;
+ public Transform3d[] getRobotToCameraTransforms() {
+ return new Transform3d[] {ROBOT_TO_CAMERA_0};
}
@Override
@@ -216,6 +227,21 @@ public double getRobotMaxVelocity() {
return MAX_VELOCITY_METERS_PER_SECOND;
}
+ @Override
+ public double getRobotMaxDriveAcceleration() {
+ return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotMaxTurnAcceleration() {
+ return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED;
+ }
+
+ @Override
+ public double getRobotSlowModeMultiplier() {
+ return SLOW_MODE_MULTIPLIER;
+ }
+
@Override
public double getRobotMaxCoastVelocity() {
return MAX_COAST_VELOCITY_METERS_PER_SECOND;
@@ -267,8 +293,8 @@ public String getCANBusName() {
}
@Override
- public String getCameraName() {
- return CAMERA_NAME;
+ public String[] getCameraNames() {
+ return new String[] {CAMERA_NAME};
}
@Override
@@ -290,4 +316,56 @@ public int getRevHighPressureSensorChannel() {
public int getRevLowPressureSensorChannel() {
return REV_LOW_PRESSURE_SENSOR_CHANNEL;
}
+
+ @Override
+ public double getDriveToPoseDriveKP() {
+ return DRIVE_TO_POSE_DRIVE_KP;
+ }
+
+ @Override
+ public double getDriveToPoseDriveKD() {
+ return DRIVE_TO_POSE_DRIVE_KD;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKP() {
+ return DRIVE_TO_POSE_THETA_KP;
+ }
+
+ @Override
+ public double getDriveToPoseThetaKD() {
+ return DRIVE_TO_POSE_THETA_KD;
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxVelocity() {
+ return getAutoMaxSpeed();
+ }
+
+ @Override
+ public double getDriveToPoseDriveMaxAcceleration() {
+ return getAutoMaxAcceleration();
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxVelocity() {
+ return getDriveToPoseDriveMaxVelocity()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseTurnMaxAcceleration() {
+ return getDriveToPoseDriveMaxAcceleration()
+ / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0);
+ }
+
+ @Override
+ public double getDriveToPoseDriveTolerance() {
+ return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS;
+ }
+
+ @Override
+ public double getDriveToPoseThetaTolerance() {
+ return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS;
+ }
}
diff --git a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java
index 8fe7c938..c2ae54cb 100644
--- a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java
+++ b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java
@@ -7,7 +7,7 @@
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-/** Class for controlling the robot with two Xbox controllers. */
+/** Class for controlling the robot with two joysticks. */
public class DualJoysticksOI implements OperatorInterface {
private final CommandJoystick translateJoystick;
private final CommandJoystick rotateJoystick;
@@ -56,11 +56,17 @@ public Trigger getResetGyroButton() {
@Override
public Trigger getXStanceButton() {
- return translateJoystickButtons[1];
+ return rotateJoystickButtons[4];
}
@Override
public Trigger getVisionIsEnabledSwitch() {
- return rotateJoystickButtons[1];
+ // vision is always enabled with dual joysticks as there is no switch to disable
+ return new Trigger(() -> true);
+ }
+
+ @Override
+ public Trigger getTurboButton() {
+ return translateJoystickButtons[1];
}
}
diff --git a/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java
new file mode 100644
index 00000000..058734d1
--- /dev/null
+++ b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java
@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.operator_interface;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+/**
+ * Class for controlling the robot with two joysticks, 1 Xbox controller, and 1 operator button
+ * panel.
+ */
+public class FullOperatorConsoleOI implements OperatorInterface {
+ private final CommandJoystick translateJoystick;
+ private final Trigger[] translateJoystickButtons;
+
+ private final CommandJoystick rotateJoystick;
+ private final Trigger[] rotateJoystickButtons;
+
+ private final XboxController operatorController;
+
+ private final CommandJoystick operatorPanel;
+ private final Trigger[] operatorPanelButtons;
+
+ public FullOperatorConsoleOI(
+ int translatePort, int rotatePort, int operatorControllerPort, int operatorPanelPort) {
+ translateJoystick = new CommandJoystick(translatePort);
+ rotateJoystick = new CommandJoystick(rotatePort);
+ operatorController = new XboxController(operatorControllerPort);
+ operatorPanel = new CommandJoystick(operatorPanelPort);
+
+ // buttons use 1-based indexing such that the index matches the button number; leave index 0 set
+ // to null
+ this.translateJoystickButtons = new Trigger[13];
+ this.rotateJoystickButtons = new Trigger[13];
+ this.operatorPanelButtons = new Trigger[13];
+
+ for (int i = 1; i < translateJoystickButtons.length; i++) {
+ translateJoystickButtons[i] = translateJoystick.button(i);
+ rotateJoystickButtons[i] = rotateJoystick.button(i);
+ }
+ for (int i = 1; i < operatorPanelButtons.length; i++) {
+ operatorPanelButtons[i] = operatorPanel.button(i);
+ }
+ }
+
+ // Translate Joystick
+ @Override
+ public double getTranslateX() {
+ return -translateJoystick.getY();
+ }
+
+ @Override
+ public double getTranslateY() {
+ return -translateJoystick.getX();
+ }
+
+ @Override
+ public Trigger getLock180Button() {
+ return translateJoystickButtons[3];
+ }
+
+ @Override
+ public Trigger getResetGyroButton() {
+ return translateJoystickButtons[4];
+ }
+
+ @Override
+ public Trigger getFieldRelativeButton() {
+ return translateJoystickButtons[9];
+ }
+
+ // Rotate Joystick
+
+ @Override
+ public double getRotate() {
+ return -rotateJoystick.getX();
+ }
+
+ @Override
+ public Trigger getXStanceButton() {
+ return rotateJoystickButtons[4];
+ }
+
+ @Override
+ public Trigger getResetPoseToVisionButton() {
+ return rotateJoystickButtons[5];
+ }
+
+ // Operator Controller
+ @Override
+ public Trigger getInterruptAll() {
+ return new Trigger(operatorController::getStartButton);
+ }
+
+ // Operator Panel
+
+ @Override
+ public Trigger getVisionIsEnabledSwitch() {
+ return operatorPanelButtons[10];
+ }
+}
diff --git a/src/main/java/frc/robot/operator_interface/OISelector.java b/src/main/java/frc/robot/operator_interface/OISelector.java
index 114beea8..7c4b92b2 100644
--- a/src/main/java/frc/robot/operator_interface/OISelector.java
+++ b/src/main/java/frc/robot/operator_interface/OISelector.java
@@ -9,9 +9,12 @@
package frc.robot.operator_interface;
import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.lib.team6328.util.Alert;
import frc.lib.team6328.util.Alert.AlertType;
+@java.lang.SuppressWarnings({"java:S3776"})
+
/**
* Utility class for selecting the appropriate OI implementations based on the connected joysticks.
*/
@@ -22,13 +25,23 @@ public class OISelector {
private static final Alert nonCompetitionOperatorInterfaceWarning =
new Alert("Non-competition operator controller connected.", AlertType.WARNING);
+ private static OperatorInterface oi = null;
+
private OISelector() {}
+ public static OperatorInterface getOperatorInterface() {
+ if (didJoysticksChange()) {
+ CommandScheduler.getInstance().getActiveButtonLoop().clear();
+ oi = findOperatorInterface();
+ }
+ return oi;
+ }
+
/**
* Returns whether the connected joysticks have changed since the last time this method was
* called.
*/
- public static boolean didJoysticksChange() {
+ private static boolean didJoysticksChange() {
boolean joysticksChanged = false;
for (int port = 0; port < DriverStation.kJoystickPorts; port++) {
String name = DriverStation.getJoystickName(port);
@@ -44,10 +57,11 @@ public static boolean didJoysticksChange() {
* Instantiates and returns an appropriate OperatorInterface object based on the connected
* joysticks.
*/
- public static OperatorInterface findOperatorInterface() {
+ private static OperatorInterface findOperatorInterface() {
Integer firstPort = null;
Integer secondPort = null;
Integer xBoxPort = null;
+ Integer thirdPort = null;
for (int port = 0; port < DriverStation.kJoystickPorts; port++) {
if (DriverStation.getJoystickName(port).toLowerCase().contains("xbox")) {
if (xBoxPort == null) {
@@ -58,13 +72,19 @@ public static OperatorInterface findOperatorInterface() {
firstPort = port;
} else if (secondPort == null) {
secondPort = port;
+ } else { // thirdPort == null
+ thirdPort = port;
}
}
}
- if (firstPort != null && secondPort != null) {
+ if (firstPort != null && secondPort != null && xBoxPort != null && thirdPort != null) {
noOperatorInterfaceWarning.set(false);
nonCompetitionOperatorInterfaceWarning.set(false);
+ return new FullOperatorConsoleOI(firstPort, secondPort, xBoxPort, thirdPort);
+ } else if (firstPort != null && secondPort != null) {
+ noOperatorInterfaceWarning.set(false);
+ nonCompetitionOperatorInterfaceWarning.set(true);
return new DualJoysticksOI(firstPort, secondPort);
} else if (xBoxPort != null) {
noOperatorInterfaceWarning.set(false);
@@ -72,7 +92,7 @@ public static OperatorInterface findOperatorInterface() {
return new SingleHandheldOI(xBoxPort);
} else {
noOperatorInterfaceWarning.set(true);
- nonCompetitionOperatorInterfaceWarning.set(false);
+ nonCompetitionOperatorInterfaceWarning.set(true);
return new OperatorInterface() {};
}
}
diff --git a/src/main/java/frc/robot/operator_interface/OperatorInterface.java b/src/main/java/frc/robot/operator_interface/OperatorInterface.java
index ff30aad2..e5b69b64 100644
--- a/src/main/java/frc/robot/operator_interface/OperatorInterface.java
+++ b/src/main/java/frc/robot/operator_interface/OperatorInterface.java
@@ -8,7 +8,7 @@
package frc.robot.operator_interface;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.button.*;
/** Interface for all driver and operator controls. */
public interface OperatorInterface {
@@ -33,11 +33,35 @@ public default Trigger getResetGyroButton() {
return new Trigger(() -> false);
}
+ public default Trigger getResetPoseToVisionButton() {
+ return new Trigger(() -> false);
+ }
+
public default Trigger getXStanceButton() {
return new Trigger(() -> false);
}
+ public default Trigger getTranslationSlowModeButton() {
+ return new Trigger(() -> false);
+ }
+
+ public default Trigger getRotationSlowModeButton() {
+ return new Trigger(() -> false);
+ }
+
+ public default Trigger getLock180Button() {
+ return new Trigger(() -> false);
+ }
+
public default Trigger getVisionIsEnabledSwitch() {
return new Trigger(() -> false);
}
+
+ public default Trigger getTurboButton() {
+ return new Trigger(() -> false);
+ }
+
+ public default Trigger getInterruptAll() {
+ return new Trigger(() -> false);
+ }
}
diff --git a/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java b/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java
index e26439c0..d3a71345 100644
--- a/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java
+++ b/src/main/java/frc/robot/operator_interface/SingleHandheldOI.java
@@ -45,8 +45,19 @@ public Trigger getXStanceButton() {
return new Trigger(controller::getYButton);
}
+ @Override
+ public Trigger getTranslationSlowModeButton() {
+ return new Trigger(controller::getLeftBumper);
+ }
+
+ @Override
+ public Trigger getRotationSlowModeButton() {
+ return new Trigger(controller::getRightBumper);
+ }
+
@Override
public Trigger getVisionIsEnabledSwitch() {
- return new Trigger(controller::getXButton);
+ // vision is always enabled with Xbox as there is no switch to disable
+ return new Trigger(() -> true);
}
}
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index e9c7d92a..a5ba060b 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -27,7 +27,10 @@
import frc.lib.team3061.gyro.GyroIOInputsAutoLogged;
import frc.lib.team3061.swerve.SwerveModule;
import frc.lib.team3061.util.RobotOdometry;
+import frc.lib.team6328.util.Alert;
+import frc.lib.team6328.util.Alert.AlertType;
import frc.lib.team6328.util.TunableNumber;
+import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;
/**
@@ -36,6 +39,7 @@
* robot's rotation.
*/
public class Drivetrain extends SubsystemBase {
+
private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
@@ -88,6 +92,9 @@ public class Drivetrain extends SubsystemBase {
private boolean isFieldRelative;
+ private boolean isTranslationSlowMode = false;
+ private boolean isRotationSlowMode = false;
+
private double gyroOffset;
private ChassisSpeeds chassisSpeeds;
@@ -98,11 +105,28 @@ public class Drivetrain extends SubsystemBase {
private final SwerveDrivePoseEstimator poseEstimator;
private boolean brakeMode;
+ private Timer brakeModeTimer = new Timer();
+ private static final double BREAK_MODE_DELAY_SEC = 10.0;
private DriveMode driveMode = DriveMode.NORMAL;
private double characterizationVoltage = 0.0;
- /** Constructs a new DrivetrainSubsystem object. */
+ private boolean isTurbo;
+
+ private boolean isMoveToPoseEnabled;
+
+ private Alert noPoseAlert =
+ new Alert("Attempted to reset pose from vision, but no pose was found.", AlertType.WARNING);
+
+ /**
+ * Creates a new Drivetrain subsystem.
+ *
+ * @param gyroIO the abstracted interface for the gyro for the drivetrain
+ * @param flModule the front left swerve module
+ * @param frModule the front right swerve module
+ * @param blModule the back left swerve module
+ * @param brModule the back right swerve module
+ */
public Drivetrain(
GyroIO gyroIO,
SwerveModule flModule,
@@ -121,7 +145,7 @@ public Drivetrain(
this.zeroGyroscope();
- this.isFieldRelative = false;
+ this.isFieldRelative = true;
this.gyroOffset = 0;
@@ -129,10 +153,21 @@ public Drivetrain(
this.poseEstimator = RobotOdometry.getInstance().getPoseEstimator();
+ // based on testing we can drive in turbo mode all the time
+ this.isTurbo = true;
+
+ this.isMoveToPoseEnabled = true;
+
ShuffleboardTab tabMain = Shuffleboard.getTab("MAIN");
- tabMain.addNumber("Gyroscope Angle", () -> getRotation().getDegrees());
- tabMain.addBoolean("X-Stance On?", this::isXstance);
- tabMain.addBoolean("Field-Relative Enabled?", () -> this.isFieldRelative);
+ tabMain
+ .addNumber("Gyroscope Angle", () -> getRotation().getDegrees())
+ .withPosition(9, 0)
+ .withSize(1, 1);
+ tabMain.addBoolean("X-Stance On?", this::isXstance).withPosition(7, 0).withSize(1, 1);
+ tabMain
+ .addBoolean("Field-Relative Enabled?", () -> this.isFieldRelative)
+ .withPosition(8, 0)
+ .withSize(1, 1);
if (DEBUGGING) {
ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME);
@@ -154,9 +189,30 @@ public Drivetrain(
}
}
+ /** Enables "turbo" mode (i.e., acceleration is not limited by software) */
+ public void enableTurbo() {
+ this.isTurbo = true;
+ }
+
+ /** Disables "turbo" mode (i.e., acceleration is limited by software) */
+ public void disableTurbo() {
+ this.isTurbo = false;
+ }
+
+ /**
+ * Returns true if the robot is in "turbo" mode (i.e., acceleration is not limited by software);
+ * false otherwise
+ *
+ * @return true if the robot is in "turbo" mode (i.e., acceleration is not limited by software);
+ * false otherwise
+ */
+ public boolean getTurbo() {
+ return this.isTurbo;
+ }
+
/**
* Zeroes the gyroscope. This sets the current rotation of the robot to zero degrees. This method
- * is intended to be invoked only when the alignment beteween the robot's rotation and the gyro is
+ * is intended to be invoked only when the alignment between the robot's rotation and the gyro is
* sufficiently different to make field-relative driving difficult. The robot needs to be
* positioned facing away from the driver, ideally aligned to a field wall before this method is
* invoked.
@@ -173,14 +229,43 @@ public void zeroGyroscope() {
*
* @return the rotation of the robot
*/
- private Rotation2d getRotation() {
+ public Rotation2d getRotation() {
if (gyroInputs.connected) {
- return Rotation2d.fromDegrees(gyroInputs.positionDeg + this.gyroOffset);
+ return Rotation2d.fromDegrees(gyroInputs.yawDeg + this.gyroOffset);
} else {
return estimatedPoseWithoutGyro.getRotation();
}
}
+ /**
+ * Returns the yaw of the drivetrain as reported by the gyro in degrees. This value does not
+ * include the local offset and will likely be different than value returned by the getRotation
+ * method, which should probably be invoked instead.
+ *
+ * @return the yaw of the drivetrain as reported by the gyro in degrees
+ */
+ public double getYaw() {
+ return gyroInputs.yawDeg;
+ }
+
+ /**
+ * Returns the pitch of the drivetrain as reported by the gyro in degrees.
+ *
+ * @return the pitch of the drivetrain as reported by the gyro in degrees
+ */
+ public double getPitch() {
+ return gyroInputs.pitchDeg;
+ }
+
+ /**
+ * Returns the roll of the drivetrain as reported by the gyro in degrees.
+ *
+ * @return the roll of the drivetrain as reported by the gyro in degrees
+ */
+ public double getRoll() {
+ return gyroInputs.rollDeg;
+ }
+
/**
* Sets the rotation of the robot to the specified value. This method should only be invoked when
* the rotation of the robot is known (e.g., at the start of an autonomous path). Zero degrees is
@@ -193,7 +278,7 @@ public void setGyroOffset(double expectedYaw) {
// taking effect. As a result, it is recommended to never set the yaw and
// adjust the local offset instead.
if (gyroInputs.connected) {
- this.gyroOffset = expectedYaw - gyroInputs.positionDeg;
+ this.gyroOffset = expectedYaw - gyroInputs.yawDeg;
} else {
this.gyroOffset = 0;
this.estimatedPoseWithoutGyro =
@@ -238,6 +323,11 @@ public void resetOdometry(PathPlannerState state) {
new Pose2d(state.poseMeters.getTranslation(), state.holonomicRotation));
}
+ /**
+ * Sets the robot's odometry's rotation based on the gyro. This method is intended to be invoked
+ * when the vision subsystem is first disabled and may have negatively impacted the pose
+ * estimator.
+ */
public void resetPoseRotationToGyro() {
for (int i = 0; i < 4; i++) {
swerveModulePositions[i] = swerveModules[i].getPosition();
@@ -248,6 +338,25 @@ public void resetPoseRotationToGyro() {
swerveModulePositions,
new Pose2d(this.getPose().getTranslation(), this.getRotation()));
}
+
+ /**
+ * Sets the odometry of the robot based on the supplied pose (e.g., from the vision subsystem).
+ * When testing, the robot can be positioned in front of an AprilTag and this method can be
+ * invoked to reset the robot's pose based on tag.
+ *
+ * @param poseSupplier the supplier of the pose to which set the robot's odometry
+ */
+ public void resetPoseToVision(Supplier If the drive mode is XSTANCE, the robot will ignore the specified velocities and turn the
- * swerve modules into the x-stance orientation.
+ * If the translation or rotation slow mode features are enabled, the corresponding velocities
+ * will be scaled to enable finer control.
+ *
+ * If the drive mode is X, the robot will ignore the specified velocities and turn the swerve
+ * modules into the x-stance orientation.
*
* If the drive mode is CHARACTERIZATION, the robot will ignore the specified velocities and
- * run the characterization routine.
+ * run the characterization routine. Refer to the FeedForwardCharacterization command class for
+ * more information.
*
- * @param translationXSupplier the desired velocity in the x direction (m/s)
- * @param translationYSupplier the desired velocity in the y direction (m/s)
- * @param rotationSupplier the desired rotational velcoity (rad/s)
+ * @param xVelocity the desired velocity in the x direction (m/s)
+ * @param yVelocity the desired velocity in the y direction (m/s)
+ * @param rotationalVelocity the desired rotational velocity (rad/s)
+ * @param isOpenLoop true for open-loop control; false for closed-loop control
+ * @param overrideFieldRelative true to force field-relative motion; false to use the current
+ * setting
*/
public void drive(
- double xVelocity, double yVelocity, double rotationalVelocity, boolean isOpenLoop) {
+ double xVelocity,
+ double yVelocity,
+ double rotationalVelocity,
+ boolean isOpenLoop,
+ boolean overrideFieldRelative) {
switch (driveMode) {
case NORMAL:
- if (isFieldRelative) {
+ // get the slow-mode multiplier from the config
+ double slowModeMultiplier = RobotConfig.getInstance().getRobotSlowModeMultiplier();
+ // if translation or rotation is in slow mode, multiply the x and y velocities by the
+ // slow-mode multiplier
+ if (isTranslationSlowMode) {
+ xVelocity *= slowModeMultiplier;
+ yVelocity *= slowModeMultiplier;
+ }
+ // if rotation is in slow mode, multiply the rotational velocity by the slow-mode multiplier
+ if (isRotationSlowMode) {
+ rotationalVelocity *= slowModeMultiplier;
+ }
+
+ if (isFieldRelative || overrideFieldRelative) {
chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
xVelocity, yVelocity, rotationalVelocity, getRotation());
@@ -282,11 +415,11 @@ public void drive(
}
Logger.getInstance()
- .recordOutput("Drivetrain/chassisSpeedVx", chassisSpeeds.vxMetersPerSecond);
+ .recordOutput("Drivetrain/ChassisSpeedVx", chassisSpeeds.vxMetersPerSecond);
Logger.getInstance()
- .recordOutput("Drivetrain/chassisSpeedVy", chassisSpeeds.vyMetersPerSecond);
+ .recordOutput("Drivetrain/ChassisSpeedVy", chassisSpeeds.vyMetersPerSecond);
Logger.getInstance()
- .recordOutput("Drivetrain/chassisSpeedVo", chassisSpeeds.omegaRadiansPerSecond);
+ .recordOutput("Drivetrain/ChassisSpeedVo", chassisSpeeds.omegaRadiansPerSecond);
SwerveModuleState[] swerveModuleStates =
kinematics.toSwerveModuleStates(chassisSpeeds, centerGravity);
@@ -308,7 +441,6 @@ public void drive(
case X:
this.setXStance();
-
break;
}
}
@@ -334,17 +466,19 @@ public void periodic() {
// update and log gyro inputs
gyroIO.updateInputs(gyroInputs);
- Logger.getInstance().processInputs("Drive/Gyro", gyroInputs);
+ Logger.getInstance().processInputs("Drivetrain/Gyro", gyroInputs);
- // update and log the swerve moudles inputs
+ // update and log the swerve modules inputs
for (SwerveModule swerveModule : swerveModules) {
swerveModule.updateAndProcessInputs();
}
+ Logger.getInstance().recordOutput("Drivetrain/AvgDriveCurrent", this.getAverageDriveCurrent());
- // update estimated poses
+ // update swerve module states and positions
SwerveModuleState[] states = new SwerveModuleState[4];
for (int i = 0; i < 4; i++) {
states[i] = swerveModules[i].getState();
+ prevSwerveModulePositions[i] = swerveModulePositions[i];
swerveModulePositions[i] = swerveModules[i].getPosition();
}
@@ -367,6 +501,7 @@ public void periodic() {
estimatedPoseWithoutGyro = estimatedPoseWithoutGyro.exp(twist);
}
+ // update the pose estimator based on the gyro and swerve module positions
poseEstimator.updateWithTime(
Timer.getFPGATimestamp(), this.getRotation(), swerveModulePositions);
@@ -388,39 +523,7 @@ public void periodic() {
Logger.getInstance().recordOutput("Odometry/Robot", poseEstimatorPose);
Logger.getInstance().recordOutput("3DField", new Pose3d(poseEstimatorPose));
Logger.getInstance().recordOutput("SwerveModuleStates", states);
- Logger.getInstance().recordOutput(SUBSYSTEM_NAME + "/gyroOffset", this.gyroOffset);
- }
-
- /**
- * If the robot is enabled and brake mode is not enabled, enable it. If the robot is disabled, has
- * stopped moving, and brake mode is enabled, disable it.
- */
- private void updateBrakeMode() {
- if (DriverStation.isEnabled() && !brakeMode) {
- brakeMode = true;
- setBrakeMode(true);
-
- } else {
- boolean stillMoving = false;
- for (SwerveModule mod : swerveModules) {
- if (Math.abs(mod.getState().speedMetersPerSecond)
- > RobotConfig.getInstance().getRobotMaxCoastVelocity()) {
- stillMoving = true;
- }
- }
-
- if (brakeMode && !stillMoving) {
- brakeMode = false;
- setBrakeMode(false);
- }
- }
- }
-
- private void setBrakeMode(boolean enable) {
- for (SwerveModule mod : swerveModules) {
- mod.setAngleBrakeMode(enable);
- mod.setDriveBrakeMode(enable);
- }
+ Logger.getInstance().recordOutput("Drivetrain/GyroOffset", this.gyroOffset);
}
/**
@@ -466,10 +569,42 @@ public void disableFieldRelative() {
this.isFieldRelative = false;
}
+ /**
+ * Enables slow mode for translation. When enabled, the robot's translational velocities will be
+ * scaled down.
+ */
+ public void enableTranslationSlowMode() {
+ this.isTranslationSlowMode = true;
+ }
+
+ /**
+ * Disables slow mode for translation. When disabled, the robot's translational velocities will
+ * not be scaled.
+ */
+ public void disableTranslationSlowMode() {
+ this.isTranslationSlowMode = false;
+ }
+
+ /**
+ * Enables slow mode for rotation. When enabled, the robot's rotational velocity will be scaled
+ * down.
+ */
+ public void enableRotationSlowMode() {
+ this.isRotationSlowMode = true;
+ }
+
+ /**
+ * Disables slow mode for rotation. When disabled, the robot's rotational velocity will not be
+ * scaled.
+ */
+ public void disableRotationSlowMode() {
+ this.isRotationSlowMode = false;
+ }
+
/**
* Sets the swerve modules in the x-stance orientation. In this orientation the wheels are aligned
- * to make an 'X'. This makes it more difficult for other robots to push the robot, which is
- * useful when shooting.
+ * to make an 'X'. This prevents the robot from rolling on an inclined surface and makes it more
+ * difficult for other robots to push the robot, which is useful when shooting.
*/
public void setXStance() {
chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
@@ -484,6 +619,31 @@ public void setXStance() {
}
}
+ /**
+ * Puts the drivetrain into the x-stance orientation. In this orientation the wheels are aligned
+ * to make an 'X'. This prevents the robot from rolling on an inclined surface and makes it more
+ * difficult for other robots to push the robot, which is useful when shooting. The robot cannot
+ * be driven until x-stance is disabled.
+ */
+ public void enableXstance() {
+ this.driveMode = DriveMode.X;
+ this.setXStance();
+ }
+
+ /** Disables x-stance, allowing the robot to be driven. */
+ public void disableXstance() {
+ this.driveMode = DriveMode.NORMAL;
+ }
+
+ /**
+ * Returns true if the robot is in the x-stance orientation.
+ *
+ * @return true if the robot is in the x-stance orientation
+ */
+ public boolean isXstance() {
+ return this.driveMode == DriveMode.X;
+ }
+
/**
* Sets the robot's center of gravity about which it will rotate. The origin is at the center of
* the robot. The positive x direction is forward; the positive y direction, left.
@@ -491,13 +651,13 @@ public void setXStance() {
* @param x the x coordinate of the robot's center of gravity (in meters)
* @param y the y coordinate of the robot's center of gravity (in meters)
*/
- public void setCenterGrav(double x, double y) {
+ public void setCenterGravity(double x, double y) {
this.centerGravity = new Translation2d(x, y);
}
/** Resets the robot's center of gravity about which it will rotate to the center of the robot. */
- public void resetCenterGrav() {
- setCenterGrav(0.0, 0.0);
+ public void resetCenterGravity() {
+ setCenterGravity(0.0, 0.0);
}
/**
@@ -519,27 +679,16 @@ public double getVelocityY() {
}
/**
- * Puts the drivetrain into the x-stance orientation. In this orientation the wheels are aligned
- * to make an 'X'. This makes it more difficult for other robots to push the robot, which is
- * useful when shooting. The robot cannot be driven until x-stance is disabled.
- */
- public void enableXstance() {
- this.driveMode = DriveMode.X;
- this.setXStance();
- }
-
- /** Disables the x-stance, allowing the robot to be driven. */
- public void disableXstance() {
- this.driveMode = DriveMode.NORMAL;
- }
-
- /**
- * Returns true if the robot is in the x-stance orientation.
+ * Returns the average current of the swerve module drive motors in amps.
*
- * @return true if the robot is in the x-stance orientation
+ * @return the average current of the swerve module drive motors in amps
*/
- public boolean isXstance() {
- return this.driveMode == DriveMode.X;
+ public double getAverageDriveCurrent() {
+ double totalCurrent = 0.0;
+ for (SwerveModule module : swerveModules) {
+ totalCurrent += Math.abs(module.getDriveCurrent());
+ }
+ return totalCurrent / swerveModules.length;
}
/**
@@ -569,16 +718,24 @@ public PIDController getAutoThetaController() {
return autoThetaController;
}
- /** Runs forwards at the commanded voltage. */
+ /**
+ * Runs forwards at the commanded voltage.
+ *
+ * @param volts the commanded voltage
+ */
public void runCharacterizationVolts(double volts) {
driveMode = DriveMode.CHARACTERIZATION;
characterizationVoltage = volts;
// invoke drive which will set the characterization voltage to each module
- drive(0, 0, 0, true);
+ drive(0, 0, 0, true, false);
}
- /** Returns the average drive velocity in meters/sec. */
+ /**
+ * Returns the average drive velocity in meters/sec.
+ *
+ * @return the average drive velocity in meters/sec
+ */
public double getCharacterizationVelocity() {
double driveVelocityAverage = 0.0;
for (SwerveModule swerveModule : swerveModules) {
@@ -587,6 +744,59 @@ public double getCharacterizationVelocity() {
return driveVelocityAverage / 4.0;
}
+ /**
+ * Enables or disables the move-to-pose feature. Refer to the MoveToPose command class for more
+ * information.
+ *
+ * @param state true to enable, false to disable
+ */
+ public void enableMoveToPose(boolean state) {
+ this.isMoveToPoseEnabled = state;
+ }
+
+ /**
+ * Returns true if the move-to-pose feature is enabled; false otherwise.
+ *
+ * @return true if the move-to-pose feature is enabled; false otherwise
+ */
+ public boolean isMoveToPoseEnabled() {
+ return this.isMoveToPoseEnabled;
+ }
+
+ /**
+ * If the robot is enabled and brake mode is not enabled, enable it. If the robot is disabled, has
+ * stopped moving for the specified period of time, and brake mode is enabled, disable it.
+ */
+ private void updateBrakeMode() {
+ if (DriverStation.isEnabled() && !brakeMode) {
+ brakeMode = true;
+ setBrakeMode(true);
+ brakeModeTimer.restart();
+
+ } else {
+ boolean stillMoving = false;
+ for (SwerveModule mod : swerveModules) {
+ if (Math.abs(mod.getState().speedMetersPerSecond)
+ > RobotConfig.getInstance().getRobotMaxCoastVelocity()) {
+ stillMoving = true;
+ brakeModeTimer.restart();
+ }
+ }
+
+ if (brakeMode && !stillMoving && brakeModeTimer.hasElapsed(BREAK_MODE_DELAY_SEC)) {
+ brakeMode = false;
+ setBrakeMode(false);
+ }
+ }
+ }
+
+ private void setBrakeMode(boolean enable) {
+ for (SwerveModule mod : swerveModules) {
+ mod.setAngleBrakeMode(enable);
+ mod.setDriveBrakeMode(enable);
+ }
+ }
+
private enum DriveMode {
NORMAL,
X,
diff --git a/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java
new file mode 100644
index 00000000..152fc173
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/subsystem/Subsystem.java
@@ -0,0 +1,95 @@
+package frc.robot.subsystems.subsystem;
+
+import static frc.robot.subsystems.subsystem.SubsystemConstants.*;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.team6328.util.TunableNumber;
+import org.littletonrobotics.junction.Logger;
+
+/**
+ * Models a generic subsystem for a rotational mechanism. The other subsystems defined in this
+ * library aren't good examples for typical robot subsystems. This class can serve as an example or
+ * be used for quick prototyping.
+ */
+public class Subsystem extends SubsystemBase {
+
+ // these Tunables are convenient when testing as they provide direct control of the subsystem's
+ // motor
+ private final TunableNumber motorPower = new TunableNumber("Subsystem/power", 0.0);
+ private final TunableNumber motorCurrent = new TunableNumber("Subsystem/current", 0.0);
+ private final TunableNumber motorPosition = new TunableNumber("Subsystem/position", 0.0);
+
+ private final SubsystemIOInputsAutoLogged inputs = new SubsystemIOInputsAutoLogged();
+ private SubsystemIO io;
+
+ /**
+ * Create a new subsystem with its associated hardware interface object.
+ *
+ * @param io the hardware interface object for this subsystem
+ */
+ public Subsystem(SubsystemIO io) {
+
+ this.io = io;
+
+ // Create a Shuffleboard tab for this subsystem if testing is enabled. Add additional indicators
+ // and controls as needed.
+ if (TESTING) {
+ ShuffleboardTab tab = Shuffleboard.getTab(SUBSYSTEM_NAME);
+ tab.add(SUBSYSTEM_NAME, this);
+ }
+ }
+
+ /**
+ * The subsystem's periodic method needs to update and process the inputs from the hardware
+ * interface object.
+ */
+ @Override
+ public void periodic() {
+ io.updateInputs(inputs);
+ Logger.getInstance().processInputs("Subsystem", inputs);
+
+ // when testing, set the motor power, current, or position based on the Tunables (if non-zero)
+ if (TESTING) {
+ if (motorPower.get() != 0) {
+ this.setMotorPower(motorPower.get());
+ }
+
+ if (motorCurrent.get() != 0) {
+ this.setMotorCurrent(motorCurrent.get());
+ }
+
+ if (motorPosition.get() != 0) {
+ this.setMotorPosition(motorPosition.get());
+ }
+ }
+ }
+
+ /**
+ * Set the motor power to the specified percentage of maximum power.
+ *
+ * @param power the percentage of maximum power to set the motor to
+ */
+ public void setMotorPower(double power) {
+ io.setMotorPower(power);
+ }
+
+ /**
+ * Set the motor current to the specified value in amps.
+ *
+ * @param power the current to set the motor to in amps
+ */
+ public void setMotorCurrent(double power) {
+ io.setMotorCurrent(power);
+ }
+
+ /**
+ * Set the motor position to the specified value in degrees.
+ *
+ * @param position the position to set the motor to in degrees
+ */
+ public void setMotorPosition(double position) {
+ io.setMotorPosition(position, POSITION_FEEDFORWARD);
+ }
+}
diff --git a/src/main/java/frc/robot/subsystems/subsystem/SubsystemConstants.java b/src/main/java/frc/robot/subsystems/subsystem/SubsystemConstants.java
new file mode 100644
index 00000000..592e87fb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/subsystem/SubsystemConstants.java
@@ -0,0 +1,36 @@
+package frc.robot.subsystems.subsystem;
+
+import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
+
+public class SubsystemConstants {
+
+ private static final String CONSTRUCTOR_EXCEPTION = "constant class";
+
+ private SubsystemConstants() {
+ throw new IllegalStateException(CONSTRUCTOR_EXCEPTION);
+ }
+
+ public static final boolean DEBUGGING = true;
+ public static final boolean TESTING = true;
+ public static final String SUBSYSTEM_NAME = "Subsystem";
+
+ public static final int MOTOR_CAN_ID = 6;
+ public static final double GEAR_RATIO = 100.0;
+ public static final boolean MOTOR_INVERTED = false;
+
+ public static final double POSITION_PID_F = 0.0;
+ public static final double POSITION_PID_P = 0.0;
+ public static final double POSITION_PID_I = 0;
+ public static final double POSITION_PID_D = 0;
+ public static final double POSITION_PID_I_ZONE = 0;
+ public static final double POSITION_PID_PEAK_OUTPUT = 1.0;
+ public static final double POSITION_FEEDFORWARD = 0;
+
+ public static final double CURRENT_LIMIT = 40;
+
+ public static final StatorCurrentLimitConfiguration CURRENT_LIMIT_CONFIG =
+ new StatorCurrentLimitConfiguration(true, CURRENT_LIMIT, 50, 0.5);
+
+ public static final int TIMEOUT_MS = 30;
+ public static final int SLOT_INDEX = 0;
+}
diff --git a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIO.java b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIO.java
new file mode 100644
index 00000000..493825f5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIO.java
@@ -0,0 +1,46 @@
+package frc.robot.subsystems.subsystem;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/** Generic subsystem hardware interface. */
+public interface SubsystemIO {
+ /** Contains all of the input data received from hardware. */
+ @AutoLog
+ public static class SubsystemIOInputs {
+ double positionDeg = 0.0;
+ double velocityRPM = 0.0;
+ double closedLoopError = 0.0;
+ double appliedVoltage = 0.0;
+ double setpoint = 0.0;
+ double power = 0.0;
+ String controlMode = "";
+ double[] statorCurrentAmps = new double[] {};
+ double[] tempCelsius = new double[] {};
+ double[] supplyCurrentAmps = new double[] {};
+ }
+
+ /** Updates the set of loggable inputs. */
+ public default void updateInputs(SubsystemIOInputs inputs) {}
+
+ /**
+ * Set the motor power to the specified percentage of maximum power.
+ *
+ * @param power the percentage of maximum power to set the motor to
+ */
+ public default void setMotorPower(double power) {}
+
+ /**
+ * Set the motor current to the specified value in amps.
+ *
+ * @param power the current to set the motor to in amps
+ */
+ public default void setMotorCurrent(double current) {}
+
+ /**
+ * Set the motor position to the specified value in degrees.
+ *
+ * @param position the position to set the motor to in degrees
+ * @param arbitraryFeedForward the arbitrary feed forward as a percentage of maximum power
+ */
+ public default void setMotorPosition(double position, double arbitraryFeedForward) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java
new file mode 100644
index 00000000..0a4240f6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/subsystem/SubsystemIOTalonFX.java
@@ -0,0 +1,118 @@
+package frc.robot.subsystems.subsystem;
+
+import static frc.robot.subsystems.subsystem.SubsystemConstants.*;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.DemandType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import frc.lib.team254.drivers.TalonFXFactory;
+import frc.lib.team3061.RobotConfig;
+import frc.lib.team3061.swerve.Conversions;
+import frc.lib.team6328.util.TunableNumber;
+
+/** TalonFX implementation of the generic SubsystemIO */
+public class SubsystemIOTalonFX implements SubsystemIO {
+ private TalonFX motor;
+
+ private final TunableNumber kP = new TunableNumber("Subsystem/kP", POSITION_PID_P);
+ private final TunableNumber kI = new TunableNumber("Subsystem/kI", POSITION_PID_I);
+ private final TunableNumber kD = new TunableNumber("Subsystem/kD", POSITION_PID_D);
+ private final TunableNumber kF = new TunableNumber("Subsystem/kF", POSITION_PID_F);
+ private final TunableNumber kIz = new TunableNumber("Subsystem/kIz", POSITION_PID_I_ZONE);
+ private final TunableNumber kPeakOutput =
+ new TunableNumber("Subsystem/kPeakOutput", POSITION_PID_PEAK_OUTPUT);
+
+ /** Create a TalonFX-specific generic SubsystemIO */
+ public SubsystemIOTalonFX() {
+ configMotor(MOTOR_CAN_ID);
+ }
+
+ /**
+ * Update the inputs based on the current state of the TalonFX motor controller.
+ *
+ * @param inputs the inputs object to update
+ */
+ @Override
+ public void updateInputs(SubsystemIOInputs inputs) {
+ inputs.positionDeg = Conversions.falconToDegrees(motor.getSelectedSensorPosition(), GEAR_RATIO);
+ inputs.velocityRPM = Conversions.falconToRPM(motor.getSelectedSensorVelocity(), GEAR_RATIO);
+ inputs.closedLoopError = motor.getClosedLoopError();
+ inputs.appliedVoltage = motor.getMotorOutputVoltage();
+ inputs.setpoint = motor.getClosedLoopTarget();
+ inputs.power = motor.getMotorOutputPercent();
+ inputs.controlMode = motor.getControlMode().toString();
+ inputs.statorCurrentAmps = new double[] {motor.getStatorCurrent()};
+ inputs.tempCelsius = new double[] {motor.getTemperature()};
+ inputs.supplyCurrentAmps = new double[] {motor.getSupplyCurrent()};
+
+ // update configuration if tunables have changed
+ if (kP.hasChanged()
+ || kI.hasChanged()
+ || kD.hasChanged()
+ || kF.hasChanged()
+ || kIz.hasChanged()
+ || kPeakOutput.hasChanged()) {
+ motor.config_kF(0, kF.get());
+ motor.config_kP(0, kP.get());
+ motor.config_kI(0, kI.get());
+ motor.config_kD(0, kD.get());
+ motor.config_IntegralZone(0, kIz.get());
+ motor.configClosedLoopPeakOutput(0, kPeakOutput.get());
+ }
+ }
+
+ /**
+ * Set the motor power to the specified percentage of maximum power.
+ *
+ * @param power the percentage of maximum power to set the motor to
+ */
+ @Override
+ public void setMotorPower(double power) {
+ motor.set(ControlMode.PercentOutput, power);
+ }
+
+ /**
+ * Set the motor current to the specified value in amps.
+ *
+ * @param power the current to set the motor to in amps
+ */
+ @Override
+ public void setMotorCurrent(double current) {
+ motor.set(ControlMode.Current, current);
+ }
+
+ /**
+ * Set the motor position to the specified value in degrees.
+ *
+ * @param position the position to set the motor to in degrees
+ * @param arbitraryFeedForward the arbitrary feed forward as a percentage of maximum power
+ */
+ @Override
+ public void setMotorPosition(double position, double arbitraryFeedForward) {
+ motor.set(
+ TalonFXControlMode.Position,
+ Conversions.degreesToFalcon(position, GEAR_RATIO),
+ DemandType.ArbitraryFeedForward,
+ arbitraryFeedForward);
+ }
+
+ private void configMotor(int motorID) {
+ TalonFXFactory.Configuration motorConfig = new TalonFXFactory.Configuration();
+ motorConfig.INVERTED = MOTOR_INVERTED;
+ motorConfig.BRUSHLESS_CURRENT_STATUS_FRAME_RATE_MS = 9;
+ motorConfig.NEUTRAL_MODE = NeutralMode.Brake;
+ motorConfig.STATOR_CURRENT_LIMIT = CURRENT_LIMIT_CONFIG;
+ motorConfig.SLOT0_KP = kP.get();
+ motorConfig.SLOT0_KI = kI.get();
+ motorConfig.SLOT0_KD = kD.get();
+ motorConfig.SLOT0_KF = kF.get();
+ motorConfig.NEUTRAL_DEADBAND = 0.001;
+ motor =
+ TalonFXFactory.createTalon(motorID, RobotConfig.getInstance().getCANBusName(), motorConfig);
+ motor.setSelectedSensorPosition(0);
+ motor.configClosedLoopPeakOutput(0, kPeakOutput.get());
+ motor.config_IntegralZone(0, kIz.get());
+ }
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index cae0375b..fa08705a 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,7 +1,7 @@
{
"fileName": "AdvantageKit.json",
"name": "AdvantageKit",
- "version": "2.1.2",
+ "version": "2.2.4",
"uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
"mavenUrls": [],
"jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
@@ -9,24 +9,24 @@
{
"groupId": "org.littletonrobotics.akit.junction",
"artifactId": "wpilib-shim",
- "version": "2.1.2"
+ "version": "2.2.4"
},
{
"groupId": "org.littletonrobotics.akit.junction",
"artifactId": "junction-core",
- "version": "2.1.2"
+ "version": "2.2.4"
},
{
"groupId": "org.littletonrobotics.akit.conduit",
"artifactId": "conduit-api",
- "version": "2.1.2"
+ "version": "2.2.4"
}
],
"jniDependencies": [
{
"groupId": "org.littletonrobotics.akit.conduit",
"artifactId": "conduit-wpilibio",
- "version": "2.1.2",
+ "version": "2.2.4",
"skipInvalidPlatforms": false,
"isJar": false,
"validPlatforms": [
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 53c27617..8e61586b 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib.json",
"name": "PathplannerLib",
- "version": "2023.4.1",
+ "version": "2023.4.4",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"mavenUrls": [
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
@@ -11,7 +11,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
- "version": "2023.4.1"
+ "version": "2023.4.4"
}
],
"jniDependencies": [],
@@ -19,7 +19,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
- "version": "2023.4.1",
+ "version": "2023.4.4",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,
diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
index 55bc0990..04bb6597 100644
--- a/vendordeps/Phoenix.json
+++ b/vendordeps/Phoenix.json
@@ -1,7 +1,7 @@
{
"fileName": "Phoenix.json",
"name": "CTRE-Phoenix (v5)",
- "version": "5.30.4",
+ "version": "5.30.4+23.0.11",
"frcYear": 2023,
"uuid": "ab676553-b602-441f-a38d-f1296eff6537",
"mavenUrls": [
@@ -50,7 +50,7 @@
{
"groupId": "com.ctre.phoenixpro",
"artifactId": "tools",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -63,7 +63,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "tools-sim",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -76,7 +76,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simTalonSRX",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -89,7 +89,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simTalonFX",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -102,7 +102,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simVictorSPX",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -115,7 +115,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simPigeonIMU",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -128,7 +128,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simCANCoder",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -141,7 +141,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProTalonFX",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -154,7 +154,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProCANcoder",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -167,7 +167,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProPigeon2",
- "version": "23.0.5",
+ "version": "23.0.11",
"isJar": false,
"skipInvalidPlatforms": true,
"validPlatforms": [
@@ -227,7 +227,7 @@
{
"groupId": "com.ctre.phoenixpro",
"artifactId": "tools",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_PhoenixTools",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -287,7 +287,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "tools-sim",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_PhoenixTools_Sim",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -302,7 +302,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simTalonSRX",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimTalonSRX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -317,7 +317,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simTalonFX",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -332,7 +332,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simVictorSPX",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimVictorSPX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -347,7 +347,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simPigeonIMU",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimPigeonIMU",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -362,7 +362,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simCANCoder",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimCANCoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -377,7 +377,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProTalonFX",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimProTalonFX",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -392,7 +392,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProCANcoder",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimProCANcoder",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -407,7 +407,7 @@
{
"groupId": "com.ctre.phoenixpro.sim",
"artifactId": "simProPigeon2",
- "version": "23.0.5",
+ "version": "23.0.11",
"libName": "CTRE_SimProPigeon2",
"headerClassifier": "headers",
"sharedLibrary": true,
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 4f378c49..dad31057 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,7 +1,7 @@
{
"fileName": "photonlib.json",
"name": "photonlib",
- "version": "v2023.3.0",
+ "version": "v2023.4.2",
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
"mavenUrls": [
"https://maven.photonvision.org/repository/internal",
@@ -13,7 +13,7 @@
{
"groupId": "org.photonvision",
"artifactId": "PhotonLib-cpp",
- "version": "v2023.3.0",
+ "version": "v2023.4.2",
"libName": "Photon",
"headerClassifier": "headers",
"sharedLibrary": true,
@@ -30,12 +30,12 @@
{
"groupId": "org.photonvision",
"artifactId": "PhotonLib-java",
- "version": "v2023.3.0"
+ "version": "v2023.4.2"
},
{
"groupId": "org.photonvision",
"artifactId": "PhotonTargeting-java",
- "version": "v2023.3.0"
+ "version": "v2023.4.2"
}
]
}
\ No newline at end of file