From 684fe90f0ed4308a3027e9adfbc3bf1ece0ad2a7 Mon Sep 17 00:00:00 2001 From: Geoff Schmit Date: Fri, 26 Jul 2024 13:04:35 -0500 Subject: [PATCH] back port from 2024 robot code (#101) * put tuning auto paths in PathPlanner folders * adjust PathPlanner defaults for 2024 robot * add start point auto that sets the robot's pose to the specified position * run follow path warmup command in robotInit to force loading of PathPlanner's Java classes * update to latest vendor deps * display LEDs and DIO ports in the sim GUI * add workaround from CTRE to ensure that the CANivore has been enumerated by the root hub and therefore, hoot files will be properly generated * add DEMO_MODE to Constants that slows robot's velocity and displays alternative LED patterns * add names to all scheduled commands to facilitate debugging * consolidate management of current alliance color in Field2d singleton * add ability to override rotation during PathPlanner paths (useful for targeting) * constrain the robot pose to stay on the field, which doesn't happen when the wheels slip when colliding with the field walls * reset the robot's pose to the previous pose when the pose "teleports" (i.e., changes much more than possible); why this happens rarely is still not understood but may have something to do with vision pose estimates with invalid timestamps * add accessor to get measured angular velocity of the drivetrain * add accessor to get the estimated robot pose in the future based on current velocity * tuned system test for drivetrain * add enabled inputs for drive and steer motors * fix bug where swerve states in inputs were not initialized * use new initial configs for drie and steer motors in CTRE's SwerveModuleConstantsFactory to specify current limits * fix bug where closed loop reference and error signals were not associated with current control mode and, therefore, returning invalid values * use TorqueCurrentFOC for steer control in simulation to match physical robot (CTRE fixed bug preventing this) * add constants for MK4i L3+ * tune current limits for swerve drive motors * remove swerve drive characterization autos (need to use separate project with SysId) * add auto command to characterize swerve wheel diameter based on 6328's clever technique * remove duplicate auto finished pulse in LEDs class * remove distraction LED pattern * add fire LED pattern * add orange pulse LED pattern * add support for GRB LED strips connected to roboRIO * count how many times each camera provides a new estimated pose * add ambiguity threshold for including estimated poses from vision * attempt to mitigate impact of NT4 bug where timestamps may be in the future by applying a ceiling of the current time * only provide a vision-based robot pose if the data is less than 0.5 seconds old * tune function to calculate standard deviation for vision pose estimates * add input to capture if the estimate is from seeing multiple tags * fix bug where multi-tag poses would have an ambiguity of 0, which would result in standard deviations that were too low * fix simulation bug where the PhotonPoseEstimator needs to be provided with the transform from the camera to the robot * fix simulation bug where cameras added to the vision system sim don't need to have transform applied * remove 2023 robot configurations * add 2024 Artemis robot configurations * add practice board configuration for testing off-robot with basic electrical board * add method to determine if the robot has complete left the alliance side of the field (useful in auto) * change behavior of reset pose to vision to wait until a recent vision pose estimate is available before applying * allow all physical toggle switches to trigger when disabled (avoids need to force toggle when enabled) * fix bug in DriveToPose to account for blue field origin --- .pathplanner/settings.json | 22 +- build.gradle | 2 +- simgui.json | 15 + .../deploy/pathplanner/autos/ChoreoTest.auto | 2 +- .../pathplanner/autos/DistanceTest.auto | 4 +- .../deploy/pathplanner/autos/Start Point.auto | 25 + src/main/deploy/pathplanner/autos/Tuning.auto | 2 +- .../pathplanner/paths/DistanceTest.path | 2 +- .../{StartPoint.path => Start Point.path} | 41 +- src/main/deploy/pathplanner/paths/Tuning.path | 2 +- .../lib/team3015/subsystem/FaultReporter.java | 18 +- .../lib/team3061/drivetrain/Drivetrain.java | 239 ++++++--- .../lib/team3061/drivetrain/DrivetrainIO.java | 16 +- .../team3061/drivetrain/DrivetrainIOCTRE.java | 76 +-- .../drivetrain/swerve/SwerveConstants.java | 37 +- src/main/java/frc/lib/team3061/leds/LEDs.java | 133 +++-- .../java/frc/lib/team3061/leds/LEDsRIO.java | 68 ++- .../java/frc/lib/team3061/vision/Vision.java | 47 +- .../lib/team3061/vision/VisionConstants.java | 8 +- .../frc/lib/team3061/vision/VisionIO.java | 5 +- .../team3061/vision/VisionIOPhotonVision.java | 55 +- .../frc/lib/team3061/vision/VisionIOSim.java | 51 +- src/main/java/frc/robot/Constants.java | 17 +- src/main/java/frc/robot/Field2d.java | 21 +- src/main/java/frc/robot/Robot.java | 14 +- src/main/java/frc/robot/RobotContainer.java | 235 ++++----- .../java/frc/robot/commands/DriveToPose.java | 9 +- .../WheelDiameterCharacterization.java | 91 ++++ .../frc/robot/configs/ArtemisRobotConfig.java | 474 ++++++++++++++++++ .../configs/GenericDrivetrainRobotConfig.java | 430 ++++++++++++++++ .../robot/configs/PracticeBoardConfig.java | 448 +++++++++++++++++ .../robot/configs/PracticeRobotConfig.java | 10 +- .../operator_interface/DualJoysticksOI.java | 5 - .../FullOperatorConsoleOI.java | 5 - .../operator_interface/OperatorInterface.java | 9 +- vendordeps/AdvantageKit.json | 10 +- vendordeps/PathplannerLib.json | 6 +- vendordeps/Phoenix6.json | 48 +- vendordeps/REVLib.json | 10 +- vendordeps/photonlib.json | 10 +- 40 files changed, 2270 insertions(+), 452 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Start Point.auto rename src/main/deploy/pathplanner/paths/{StartPoint.path => Start Point.path} (50%) create mode 100644 src/main/java/frc/robot/commands/WheelDiameterCharacterization.java create mode 100644 src/main/java/frc/robot/configs/ArtemisRobotConfig.java create mode 100644 src/main/java/frc/robot/configs/GenericDrivetrainRobotConfig.java create mode 100644 src/main/java/frc/robot/configs/PracticeBoardConfig.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 279b596e..8a2f524d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,12 +1,16 @@ { - "robotWidth": 0.8382, - "robotLength": 0.8382, + "robotWidth": 0.88, + "robotLength": 0.88, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 10.0, - "defaultMaxAngVel": 561.96, - "defaultMaxAngAccel": 2066.2, - "maxModuleSpeed": 3.644 + "pathFolders": [ + "Tuning" + ], + "autoFolders": [ + "Tuning" + ], + "defaultMaxVel": 3.25, + "defaultMaxAccel": 6.948, + "defaultMaxAngVel": 530.0, + "defaultMaxAngAccel": 2005.0, + "maxModuleSpeed": 3.88 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index a730a6ab..9f23defe 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id 'com.diffplug.spotless' version '6.11.0' id "com.peterabeles.gversion" version "1.10" } diff --git a/simgui.json b/simgui.json index 384c947e..69add085 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,19 @@ { + "HALProvider": { + "Addressable LEDs": { + "0": { + "columns": 35 + }, + "window": { + "visible": true + } + }, + "DIO": { + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", diff --git a/src/main/deploy/pathplanner/autos/ChoreoTest.auto b/src/main/deploy/pathplanner/autos/ChoreoTest.auto index 8d23da8e..0a30a4b1 100644 --- a/src/main/deploy/pathplanner/autos/ChoreoTest.auto +++ b/src/main/deploy/pathplanner/autos/ChoreoTest.auto @@ -20,6 +20,6 @@ ] } }, - "folder": null, + "folder": "Tuning", "choreoAuto": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DistanceTest.auto b/src/main/deploy/pathplanner/autos/DistanceTest.auto index 18af39c8..fd85e9ee 100644 --- a/src/main/deploy/pathplanner/autos/DistanceTest.auto +++ b/src/main/deploy/pathplanner/autos/DistanceTest.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.82, + "x": 2.0, "y": 4.4 }, "rotation": 0 @@ -20,6 +20,6 @@ ] } }, - "folder": null, + "folder": "Tuning", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Start Point.auto b/src/main/deploy/pathplanner/autos/Start Point.auto new file mode 100644 index 00000000..f68c167b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Start Point.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3661772442028295, + "y": 7.31444420234812 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start Point" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Tuning.auto b/src/main/deploy/pathplanner/autos/Tuning.auto index 65764812..03a1a3ee 100644 --- a/src/main/deploy/pathplanner/autos/Tuning.auto +++ b/src/main/deploy/pathplanner/autos/Tuning.auto @@ -20,6 +20,6 @@ ] } }, - "folder": null, + "folder": "Tuning", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DistanceTest.path b/src/main/deploy/pathplanner/paths/DistanceTest.path index c8876e0d..15bc3808 100644 --- a/src/main/deploy/pathplanner/paths/DistanceTest.path +++ b/src/main/deploy/pathplanner/paths/DistanceTest.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Tuning", "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/StartPoint.path b/src/main/deploy/pathplanner/paths/Start Point.path similarity index 50% rename from src/main/deploy/pathplanner/paths/StartPoint.path rename to src/main/deploy/pathplanner/paths/Start Point.path index 1ed95408..0f525754 100644 --- a/src/main/deploy/pathplanner/paths/StartPoint.path +++ b/src/main/deploy/pathplanner/paths/Start Point.path @@ -3,45 +3,39 @@ "waypoints": [ { "anchor": { - "x": 4.48, - "y": 1.08 + "x": 2.3661772442028295, + "y": 7.31444420234812 }, "prevControl": null, "nextControl": { - "x": 5.6000000000000005, - "y": 1.08 + "x": 2.4661772442028296, + "y": 7.31444420234812 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.0, - "y": 1.08 + "x": 3.076223024483964, + "y": 7.31444420234812 }, "prevControl": { - "x": 4.479309367425445, - "y": 1.08 + "x": 2.976223024483964, + "y": 7.31444420234812 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0, - "rotationDegrees": 0, - "rotateFast": false - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 10.0, - "maxAngularVelocity": 561.96, - "maxAngularAcceleration": 2066.2 + "maxVelocity": 1.0, + "maxAcceleration": 11.805, + "maxAngularVelocity": 537.0, + "maxAngularAcceleration": 2303.0 }, "goalEndState": { "velocity": 0, @@ -49,7 +43,10 @@ "rotateFast": false }, "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": true + "folder": "Tuning", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Tuning.path b/src/main/deploy/pathplanner/paths/Tuning.path index 55bda3d4..ad9f9353 100644 --- a/src/main/deploy/pathplanner/paths/Tuning.path +++ b/src/main/deploy/pathplanner/paths/Tuning.path @@ -75,7 +75,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Tuning", "previewStartingState": { "rotation": 90.0, "velocity": 0 diff --git a/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java index 0c55f140..22915540 100644 --- a/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java +++ b/src/main/java/frc/lib/team3015/subsystem/FaultReporter.java @@ -1,5 +1,6 @@ package frc.lib.team3015.subsystem; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -42,6 +43,8 @@ private static class SubsystemFaults { private final Map subsystemsFaults = new HashMap<>(); private final boolean checkErrors; + private boolean startedCTRESignalLogger = false; + private FaultReporter() { this.checkErrors = RobotBase.isReal(); setupCallbacks(); @@ -91,13 +94,15 @@ private void setupCallbacks() { .schedule( Commands.repeatingSequence( Commands.runOnce(this::checkForFaults), Commands.waitSeconds(0.25)) - .ignoringDisable(true)); + .ignoringDisable(true) + .withName("check for faults")); CommandScheduler.getInstance() .schedule( Commands.repeatingSequence( Commands.runOnce(this::publishStatus), Commands.waitSeconds(1.0)) - .ignoringDisable(true)); + .ignoringDisable(true) + .withName("publish faults")); } private void publishStatus() { @@ -185,6 +190,15 @@ public void registerHardware(String subsystemName, String label, TalonFX phoenix subsystemsFaults.getOrDefault(subsystemName, new SubsystemFaults()); subsystemFaults.hardware.add(new SelfCheckingPhoenixMotor(label, phoenixMotor)); subsystemsFaults.put(subsystemName, subsystemFaults); + + // The following is the recommended workaround from CTRE to ensure that the CANivore has been + // enumerated by the root hub and therefore, hoot files will be properly generated. + if (!this.startedCTRESignalLogger) { + this.startedCTRESignalLogger = true; + phoenixMotor.getVersion().waitForUpdate(0.5); + SignalLogger.setPath("/media/sda1"); + SignalLogger.start(); + } } public void registerHardware(String subsystemName, String label, PWMMotorController pwmMotor) { diff --git a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java index ce287fb1..222cca1c 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java +++ b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java @@ -8,6 +8,7 @@ import static frc.robot.Constants.*; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; @@ -16,6 +17,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -31,8 +33,11 @@ import frc.lib.team3061.leds.LEDs; import frc.lib.team6328.util.Alert; import frc.lib.team6328.util.Alert.AlertType; +import frc.lib.team6328.util.FieldConstants; import frc.lib.team6328.util.TunableNumber; import frc.robot.Constants; +import frc.robot.Field2d; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -86,13 +91,23 @@ public class Drivetrain extends SubsystemBase { private boolean isMoveToPoseEnabled; + private double maxVelocity; + private Alert noPoseAlert = new Alert("Attempted to reset pose from vision, but no pose was found.", AlertType.WARNING); + private static final String SYSTEM_CHECK_PREFIX = "[System Check] Swerve module "; + private static final String IS_LITERAL = " is: "; private ChassisSpeeds prevSpeeds = new ChassisSpeeds(); private double[] prevSteerVelocitiesRevPerMin = new double[4]; - private DriverStation.Alliance alliance = DriverStation.Alliance.Blue; + private DriverStation.Alliance alliance = Field2d.getInstance().getAlliance(); + + private Pose2d prevRobotPose = new Pose2d(); + private int teleportedCount = 0; + private int constrainPoseToFieldCount = 0; + + private boolean isRotationOverrideEnabled = false; /** * Creates a new Drivetrain subsystem. @@ -111,6 +126,8 @@ public Drivetrain(DrivetrainIO io) { this.isMoveToPoseEnabled = true; + this.maxVelocity = 0.5; + ShuffleboardTab tabMain = Shuffleboard.getTab("MAIN"); tabMain .addNumber("Gyroscope Angle", () -> getRotation().getDegrees()) @@ -154,6 +171,8 @@ public Drivetrain(DrivetrainIO io) { this::shouldFlipAutoPath, this // Reference to this subsystem to set requirements ); + + PPHolonomicDriveController.setRotationTargetOverride(this::getRotationTargetOverride); } public ChassisSpeeds getRobotRelativeSpeeds() { @@ -274,6 +293,7 @@ public Pose2d getPose() { */ public void resetPose(Pose2d pose) { this.io.resetPose(pose); + this.prevRobotPose = pose; } /** @@ -282,7 +302,9 @@ public void resetPose(Pose2d pose) { * estimator. */ public void resetPoseRotationToGyro() { - this.io.resetPose(new Pose2d(this.getPose().getTranslation(), this.getRotation())); + Pose2d newPose = new Pose2d(this.getPose().getTranslation(), this.getRotation()); + this.io.resetPose(newPose); + this.prevRobotPose = newPose; } /** @@ -297,6 +319,7 @@ public void resetPoseToVision(Supplier poseSupplier) { if (pose != null) { noPoseAlert.set(false); this.io.resetPose(pose.toPose2d()); + this.prevRobotPose = pose.toPose2d(); } else { noPoseAlert.set(true); } @@ -326,8 +349,7 @@ public void resetPoseToVision(Supplier poseSupplier) { * @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 + * @param isFieldRelative true to for field-relative motion; false, for robot-relative */ public void drive( double xVelocity, @@ -347,6 +369,15 @@ public void drive( yVelocity *= slowModeMultiplier; } + if (Constants.DEMO_MODE) { + double velocity = Math.sqrt(Math.pow(xVelocity, 2) + Math.pow(yVelocity, 2)); + if (velocity > this.maxVelocity) { + double scale = this.maxVelocity / velocity; + xVelocity *= scale; + yVelocity *= scale; + } + } + // if rotation is in slow mode, multiply the rotational velocity by the slow-mode multiplier if (isRotationSlowMode) { rotationalVelocity *= slowModeMultiplier; @@ -400,12 +431,24 @@ public void driveFacingAngle( xVelocity *= slowModeMultiplier; yVelocity *= slowModeMultiplier; } + + if (Constants.DEMO_MODE) { + double velocity = Math.sqrt(Math.pow(xVelocity, 2) + Math.pow(yVelocity, 2)); + if (velocity > this.maxVelocity) { + double scale = this.maxVelocity / velocity; + xVelocity *= scale; + yVelocity *= scale; + } + } + int allianceMultiplier = this.alliance == Alliance.Blue ? 1 : -1; this.io.driveFieldRelativeFacingAngle( xVelocity * allianceMultiplier, yVelocity * allianceMultiplier, targetDirection, isOpenLoop); + + Logger.recordOutput(SUBSYSTEM_NAME + "/DriveFacingAngle/targetDirection", targetDirection); } /** @@ -461,6 +504,51 @@ public void periodic() { Math.abs(this.inputs.gyro.pitchDeg) > LEDS_FALLEN_ANGLE_DEGREES || Math.abs(this.inputs.gyro.rollDeg) > LEDS_FALLEN_ANGLE_DEGREES); + // check for teleportation + if (this.inputs.drivetrain.robotPose.minus(prevRobotPose).getTranslation().getNorm() > 0.4) { + this.resetPose(prevRobotPose); + this.teleportedCount++; + Logger.recordOutput(SUBSYSTEM_NAME + "/TeleportedPose", this.inputs.drivetrain.robotPose); + Logger.recordOutput(SUBSYSTEM_NAME + "/TeleportCount", this.teleportedCount); + } else { + this.prevRobotPose = this.inputs.drivetrain.robotPose; + } + + // check for position outside the field due to slipping + if (this.inputs.drivetrain.robotPose.getX() < 0) { + this.resetPose( + new Pose2d( + 0, + this.inputs.drivetrain.robotPose.getY(), + this.inputs.drivetrain.robotPose.getRotation())); + this.constrainPoseToFieldCount++; + } else if (this.inputs.drivetrain.robotPose.getX() > FieldConstants.fieldLength) { + this.resetPose( + new Pose2d( + FieldConstants.fieldLength, + this.inputs.drivetrain.robotPose.getY(), + this.inputs.drivetrain.robotPose.getRotation())); + this.constrainPoseToFieldCount++; + } + + if (this.inputs.drivetrain.robotPose.getY() < 0) { + this.resetPose( + new Pose2d( + this.inputs.drivetrain.robotPose.getX(), + 0, + this.inputs.drivetrain.robotPose.getRotation())); + this.constrainPoseToFieldCount++; + } else if (this.inputs.drivetrain.robotPose.getY() > FieldConstants.fieldWidth) { + this.resetPose( + new Pose2d( + this.inputs.drivetrain.robotPose.getX(), + FieldConstants.fieldWidth, + this.inputs.drivetrain.robotPose.getRotation())); + this.constrainPoseToFieldCount++; + } + Logger.recordOutput( + SUBSYSTEM_NAME + "/ConstrainPoseToFieldCount", this.constrainPoseToFieldCount); + // update the brake mode based on the robot's velocity and state (enabled/disabled) updateBrakeMode(); @@ -573,23 +661,32 @@ public void resetCenterOfRotation() { } /** - * Returns the desired velocity of the drivetrain in the x direction (units of m/s) + * Returns the measured velocity of the drivetrain in the x direction (units of m/s) * - * @return the desired velocity of the drivetrain in the x direction (units of m/s) + * @return the measured velocity of the drivetrain in the x direction (units of m/s) */ public double getVelocityX() { return this.inputs.drivetrain.measuredVXMetersPerSec; } /** - * Returns the desired velocity of the drivetrain in the y direction (units of m/s) + * Returns the measured velocity of the drivetrain in the y direction (units of m/s) * - * @return the desired velocity of the drivetrain in the y direction (units of m/s) + * @return the measured velocity of the drivetrain in the y direction (units of m/s) */ public double getVelocityY() { return this.inputs.drivetrain.measuredVYMetersPerSec; } + /** + * Returns the measured rotational velocity of the drivetrain (units of rad/s) + * + * @return the measured rotational velocity of the drivetrain (units of rad/s) + */ + public double getVelocityT() { + return this.inputs.drivetrain.measuredAngularVelocityRadPerSec; + } + /** * Returns the average current of the swerve module drive motors in amps. * @@ -705,6 +802,22 @@ public double getRotateCharacterizationAcceleration() { return avgAcceleration / LOOP_PERIOD_SECS; } + /** Runs in a circle at omega. */ + public void runWheelDiameterCharacterization(double omegaSpeed) { + this.io.driveRobotRelative(0.0, 0.0, omegaSpeed, true); + } + + /** Get the position of all drive wheels in radians. */ + public double[] getWheelDiameterCharacterizationPosition() { + double[] positions = new double[inputs.swerve.length]; + for (int i = 0; i < inputs.swerve.length; i++) { + positions[i] = + inputs.swerve[i].driveDistanceMeters + / (RobotConfig.getInstance().getWheelDiameterMeters() / 2.0); + } + return positions; + } + /** * Enables or disables the move-to-pose feature. Refer to the MoveToPose command class for more * information. @@ -752,74 +865,59 @@ private void checkSwerveModule( double velocityTarget, double velocityTolerance) { - Boolean isOffset = false; + boolean isOffset = false; // Check to see if the direction is rotated properly // steerAbsolutePositionDeg is a value that is between (-180, 180] - - if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - > angleTarget - angleTolerance - && this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - < angleTarget + angleTolerance) { - } - - // check if angle is in threshold +- 180 - else if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - > angleTarget - angleTolerance - 180 - && this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - < angleTarget + angleTolerance - 180) { + if (Math.abs( + this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - (angleTarget - 180)) + < angleTolerance) { isOffset = true; - } else if (this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - > angleTarget - angleTolerance + 180 - && this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - < angleTarget + angleTolerance + 180) { + } else if (Math.abs( + this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - (angleTarget + 180)) + < angleTolerance) { isOffset = true; } // if not, add fault - else { + else if (Math.abs(this.inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg - angleTarget) + > angleTolerance) { FaultReporter.getInstance() .addFault( SUBSYSTEM_NAME, - "[System Check] Swerve module " + SYSTEM_CHECK_PREFIX + getSwerveLocation(swerveModuleNumber) + " not rotating in the threshold as expected. Should be: " + angleTarget - + " is: " + + IS_LITERAL + inputs.swerve[swerveModuleNumber].steerAbsolutePositionDeg); } // Checks the velocity of the swerve module depending on if there is an offset if (!isOffset) { - if (inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec - > velocityTarget - velocityTolerance - && inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec - < velocityTarget + velocityTolerance) { - } else { + if (Math.abs(inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec - velocityTarget) + > velocityTolerance) { FaultReporter.getInstance() .addFault( SUBSYSTEM_NAME, - "[System Check] Swerve module " + SYSTEM_CHECK_PREFIX + getSwerveLocation(swerveModuleNumber) + " not moving as fast as expected. Should be: " + velocityTarget - + " is: " + + IS_LITERAL + inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec); } } else { // if there is an offset, check the velocity in the opposite direction - if (inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec - < -(velocityTarget - velocityTolerance) - && inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec - > -(velocityTarget + velocityTolerance)) { - } else { + if (Math.abs(inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec + velocityTarget) + > velocityTolerance) { FaultReporter.getInstance() .addFault( SUBSYSTEM_NAME, - "[System Check] Swerve module " + SYSTEM_CHECK_PREFIX + getSwerveLocation(swerveModuleNumber) + " not moving as fast as expected. REVERSED Should be: " + velocityTarget - + " is: " + + IS_LITERAL + inputs.swerve[swerveModuleNumber].driveVelocityMetersPerSec); } } @@ -865,11 +963,7 @@ private Command getSwerveCheckCommand(SwerveCheckTypes type) { break; case CLOCKWISE: return Commands.parallel( - Commands.run( - () -> { - this.drive(0, 0, -Math.PI, false, false); - }, - this), + Commands.run(() -> this.drive(0, 0, -Math.PI, false, false), this), Commands.waitSeconds(1) .andThen( Commands.runOnce( @@ -882,11 +976,7 @@ private Command getSwerveCheckCommand(SwerveCheckTypes type) { .withTimeout(1); case COUNTERCLOCKWISE: return Commands.parallel( - Commands.run( - () -> { - this.drive(0, 0, Math.PI, false, false); - }, - this), + Commands.run(() -> this.drive(0, 0, Math.PI, false, false), this), Commands.waitSeconds(1) .andThen( Commands.runOnce( @@ -908,10 +998,7 @@ private Command getSwerveCheckCommand(SwerveCheckTypes type) { return Commands.parallel( Commands.run( - () -> { - this.drive(xVelocity, yVelocity, rotationalVelocity, false, false); - }, - this), + () -> this.drive(xVelocity, yVelocity, rotationalVelocity, false, false), this), Commands.waitSeconds(1) .andThen( Commands.runOnce( @@ -947,7 +1034,7 @@ public boolean shouldFlipAutoPath() { public Command getSystemCheckCommand() { return Commands.sequence( - Commands.runOnce(() -> this.disableFieldRelative(), this), + Commands.runOnce(this::disableFieldRelative, this), Commands.runOnce(() -> FaultReporter.getInstance().clearFaults(SUBSYSTEM_NAME)), getSwerveCheckCommand(SwerveCheckTypes.LEFT), getSwerveCheckCommand(SwerveCheckTypes.RIGHT), @@ -965,9 +1052,11 @@ public Command getSystemCheckCommand() { * 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); + if (DriverStation.isEnabled()) { + if (!brakeMode) { + brakeMode = true; + setBrakeMode(true); + } brakeModeTimer.restart(); } else if (!DriverStation.isEnabled()) { @@ -990,6 +1079,36 @@ private void setBrakeMode(boolean enable) { this.io.setBrakeMode(enable); } + public void enableRotationOverride() { + this.isRotationOverrideEnabled = true; + } + + public void disableRotationOverride() { + this.isRotationOverrideEnabled = false; + } + + public Optional getRotationTargetOverride() { + // Some condition that should decide if we want to override rotation + if (this.isRotationOverrideEnabled) { + Rotation2d targetRotation = new Rotation2d(); + Logger.recordOutput(SUBSYSTEM_NAME + "/rotationOverride", targetRotation); + return Optional.of(targetRotation); + } else { + // return an empty optional when we don't want to override the path's rotation + return Optional.empty(); + } + } + + public Pose2d getFutureRobotPose(double secondsInFuture) { + // project the robot pose into the future based on the current translational velocity; don't + // project the current rotational velocity as that will adversely affect the control loop + // attempting to reach the rotational setpoint. + return this.getPose() + .exp( + new Twist2d( + this.getVelocityX() * secondsInFuture, this.getVelocityY() * secondsInFuture, 0.0)); + } + private enum DriveMode { NORMAL, X diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java index 41332aca..674c40a4 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIO.java @@ -13,6 +13,7 @@ public interface DrivetrainIO { @AutoLog public static class SwerveIOInputs { + public boolean driveEnabled = false; public double driveDistanceMeters = 0.0; public double driveVelocityMetersPerSec = 0.0; public double driveVelocityReferenceMetersPerSec = 0.0; @@ -23,6 +24,7 @@ public static class SwerveIOInputs { public double driveSupplyCurrentAmps = 0.0; public double driveTempCelsius = 0.0; + public boolean steerEnabled = false; public double steerAbsolutePositionDeg = 0.0; public double steerPositionDeg = 0.0; public double steerPositionReferenceDeg = 0.0; @@ -46,8 +48,18 @@ public static class DrivetrainIOInputs { double measuredVYMetersPerSec = 0.0; double measuredAngularVelocityRadPerSec = 0.0; - SwerveModuleState[] swerveReferenceStates = new SwerveModuleState[4]; - SwerveModuleState[] swerveMeasuredStates = new SwerveModuleState[4]; + SwerveModuleState[] swerveReferenceStates = { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; + SwerveModuleState[] swerveMeasuredStates = { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }; Pose2d robotPoseWithoutGyro = new Pose2d(); Pose2d robotPose = new Pose2d(); diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java index 3d42dad0..699a8728 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java @@ -6,6 +6,8 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; @@ -16,6 +18,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.ctre.phoenix6.signals.DeviceEnableValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -132,6 +135,32 @@ public SwerveModuleSignals(TalonFX driveMotor, TalonFX steerMotor) { private static final SwerveModuleConstantsFactory constantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT) + .withPeakReverseTorqueCurrent(-SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(SwerveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT) + .withSupplyCurrentThreshold(SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT) + .withSupplyTimeThreshold(SwerveConstants.DRIVE_PEAK_CURRENT_DURATION) + .withSupplyCurrentLimitEnable(SwerveConstants.DRIVE_ENABLE_CURRENT_LIMIT) + .withStatorCurrentLimit(SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT) + .withStatorCurrentLimitEnable( + SwerveConstants.DRIVE_ENABLE_CURRENT_LIMIT))) + .withSteerMotorInitialConfigs( + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(SwerveConstants.ANGLE_CONTINUOUS_CURRENT_LIMIT) + .withSupplyCurrentThreshold(SwerveConstants.ANGLE_PEAK_CURRENT_LIMIT) + .withSupplyTimeThreshold(SwerveConstants.ANGLE_PEAK_CURRENT_DURATION) + .withSupplyCurrentLimitEnable(SwerveConstants.ANGLE_ENABLE_CURRENT_LIMIT) + .withStatorCurrentLimit(SwerveConstants.ANGLE_PEAK_CURRENT_LIMIT) + .withStatorCurrentLimitEnable( + SwerveConstants.ANGLE_ENABLE_CURRENT_LIMIT))) .withDriveMotorGearRatio( RobotConfig.getInstance().getSwerveConstants().getDriveGearRatio()) .withSteerMotorGearRatio( @@ -233,25 +262,6 @@ public DrivetrainIOCTRE() { backLeft, backRight); - // configure current limits - for (SwerveModule swerveModule : this.Modules) { - CurrentLimitsConfigs currentLimits = new CurrentLimitsConfigs(); - swerveModule.getDriveMotor().getConfigurator().refresh(currentLimits); - currentLimits.SupplyCurrentLimit = SwerveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; - currentLimits.SupplyCurrentThreshold = SwerveConstants.DRIVE_PEAK_CURRENT_LIMIT; - currentLimits.SupplyTimeThreshold = SwerveConstants.DRIVE_PEAK_CURRENT_DURATION; - currentLimits.SupplyCurrentLimitEnable = SwerveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - swerveModule.getDriveMotor().getConfigurator().apply(currentLimits); - - currentLimits = new CurrentLimitsConfigs(); - swerveModule.getSteerMotor().getConfigurator().refresh(currentLimits); - currentLimits.SupplyCurrentLimit = SwerveConstants.ANGLE_CONTINUOUS_CURRENT_LIMIT; - currentLimits.SupplyCurrentThreshold = SwerveConstants.ANGLE_PEAK_CURRENT_LIMIT; - currentLimits.SupplyTimeThreshold = SwerveConstants.ANGLE_PEAK_CURRENT_DURATION; - currentLimits.SupplyCurrentLimitEnable = SwerveConstants.ANGLE_ENABLE_CURRENT_LIMIT; - swerveModule.getSteerMotor().getConfigurator().apply(currentLimits); - } - this.pitchStatusSignal = this.m_pigeon2.getPitch().clone(); this.pitchStatusSignal.setUpdateFrequency(100); this.rollStatusSignal = this.m_pigeon2.getRoll().clone(); @@ -401,16 +411,22 @@ private void updateSwerveModuleInputs( SwerveModulePosition position = module.getPosition(false); SwerveModuleState state = module.getCurrentState(); + inputs.driveEnabled = + module.getDriveMotor().getDeviceEnable().getValue() == DeviceEnableValue.Enabled; inputs.driveDistanceMeters = position.distanceMeters; inputs.driveVelocityMetersPerSec = state.speedMetersPerSecond; + + // Retrieve the closed loop reference status signals directly from the motor in this method + // instead of retrieving in advance because the status signal returned depends on the current + // control mode. inputs.driveVelocityReferenceMetersPerSec = Conversions.falconRPSToMechanismMPS( - signals.driveVelocityReferenceStatusSignal.getValue(), + module.getDriveMotor().getClosedLoopReference().getValueAsDouble(), RobotConfig.getInstance().getWheelDiameterMeters() * Math.PI, RobotConfig.getInstance().getSwerveConstants().getDriveGearRatio()); inputs.driveVelocityErrorMetersPerSec = Conversions.falconRPSToMechanismMPS( - signals.driveVelocityErrorStatusSignal.getValue(), + module.getDriveMotor().getClosedLoopError().getValueAsDouble(), RobotConfig.getInstance().getWheelDiameterMeters() * Math.PI, RobotConfig.getInstance().getSwerveConstants().getDriveGearRatio()); inputs.driveAccelerationMetersPerSecPerSec = @@ -425,16 +441,22 @@ private void updateSwerveModuleInputs( inputs.steerAbsolutePositionDeg = module.getCANcoder().getAbsolutePosition().getValue() * 360.0; + inputs.steerEnabled = + module.getSteerMotor().getDeviceEnable().getValue() == DeviceEnableValue.Enabled; // since we are using the FusedCANcoder feature, the position and velocity signal for the angle // motor accounts for the gear ratio; so, pass a gear ratio of 1 to just convert from rotations // to degrees. inputs.steerPositionDeg = position.angle.getDegrees(); + + // Retrieve the closed loop reference status signals directly from the motor in this method + // instead of retrieving in advance because the status signal returned depends on the current + // control mode. inputs.steerPositionReferenceDeg = Conversions.falconRotationsToMechanismDegrees( - signals.steerPositionReferenceStatusSignal.getValue(), 1); + module.getSteerMotor().getClosedLoopReference().getValueAsDouble(), 1); inputs.steerPositionErrorDeg = Conversions.falconRotationsToMechanismDegrees( - signals.steerPositionErrorStatusSignal.getValue(), 1); + module.getSteerMotor().getClosedLoopError().getValueAsDouble(), 1); inputs.steerVelocityRevPerMin = Conversions.falconRPSToMechanismRPM(signals.steerVelocityStatusSignal.getValue(), 1); inputs.steerAccelerationMetersPerSecPerSec = @@ -646,9 +668,7 @@ public Pose2d updateWithTime( } private static ClosedLoopOutputType getSteerClosedLoopOutputType() { - if (Constants.getMode() == Constants.Mode.SIM) { - return ClosedLoopOutputType.Voltage; - } else if (RobotConfig.getInstance().getSwerveSteerControlMode() + if (RobotConfig.getInstance().getSwerveSteerControlMode() == RobotConfig.SWERVE_CONTROL_MODE.TORQUE_CURRENT_FOC) { return ClosedLoopOutputType.TorqueCurrentFOC; } else { @@ -657,9 +677,7 @@ private static ClosedLoopOutputType getSteerClosedLoopOutputType() { } private static ClosedLoopOutputType getDriveClosedLoopOutputType() { - if (Constants.getMode() == Constants.Mode.SIM) { - return ClosedLoopOutputType.Voltage; - } else if (RobotConfig.getInstance().getSwerveDriveControlMode() + if (RobotConfig.getInstance().getSwerveDriveControlMode() == RobotConfig.SWERVE_CONTROL_MODE.TORQUE_CURRENT_FOC) { return ClosedLoopOutputType.TorqueCurrentFOC; } else { diff --git a/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java index a1810171..49e01a43 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java @@ -101,6 +101,39 @@ public boolean isCanCoderInverted() { } }; + public static final SwerveConstants MK4I_L3_PLUS_CONSTANTS = + new SwerveConstants() { + @Override + public double getDriveGearRatio() { + return MK4I_L3_PLUS_DRIVE_GEAR_RATIO; + } + + @Override + public boolean isDriveMotorInverted() { + return MK4I_L3_DRIVE_MOTOR_INVERTED; + } + + @Override + public double getAngleGearRatio() { + return MK4I_L3_ANGLE_GEAR_RATIO; + } + + @Override + public boolean isAngleMotorInverted() { + return MK4I_L3_ANGLE_MOTOR_INVERTED; + } + + @Override + public boolean isCanCoderInverted() { + return MK4I_L3_CAN_CODER_INVERTED; + } + }; + + /* MK4i L3 */ + + private static final double MK4I_L3_PLUS_DRIVE_GEAR_RATIO = + 1 / ((16.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0)); + /* MK4i L3 */ private static final double MK4I_L3_DRIVE_GEAR_RATIO = @@ -143,8 +176,8 @@ public boolean isCanCoderInverted() { public static final double ANGLE_PEAK_CURRENT_DURATION = 0.1; public static final boolean ANGLE_ENABLE_CURRENT_LIMIT = true; - public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 50; - public static final int DRIVE_PEAK_CURRENT_LIMIT = 60; + public static final int DRIVE_CONTINUOUS_CURRENT_LIMIT = 40; + public static final int DRIVE_PEAK_CURRENT_LIMIT = 50; public static final double DRIVE_PEAK_CURRENT_DURATION = 0.1; public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; diff --git a/src/main/java/frc/lib/team3061/leds/LEDs.java b/src/main/java/frc/lib/team3061/leds/LEDs.java index 035f44f2..30beece0 100644 --- a/src/main/java/frc/lib/team3061/leds/LEDs.java +++ b/src/main/java/frc/lib/team3061/leds/LEDs.java @@ -15,6 +15,8 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.team3061.RobotConfig; +import frc.robot.Constants; +import frc.robot.Field2d; import java.util.List; @java.lang.SuppressWarnings({"java:S6548"}) @@ -35,16 +37,13 @@ public static LEDs getInstance() { // Robot state tracking private int loopCycleCount = 0; - private boolean distraction = false; private boolean fallen = false; private boolean endgameAlert = false; private boolean autoFinished = false; private double autoFinishedTime = 0.0; private boolean lowBatteryAlert = false; - private boolean demoMode = false; private boolean assignedAlliance = false; - private Alliance alliance = Alliance.Blue; private boolean lastEnabledAuto = false; private double lastEnabledTime = 0.0; private boolean estopped = false; @@ -66,16 +65,18 @@ public static LEDs getInstance() { protected static final int LENGTH = MIRROR_LEDS ? ACTUAL_LENGTH / 2 : ACTUAL_LENGTH; private static final int STATIC_LENGTH = LENGTH / 2; private static final int STATIC_SECTION_LENGTH = STATIC_LENGTH / 3; - private static final boolean PRIDE_LEDS = true; + private static final boolean PRIDE_LEDS = false; private static final int MIN_LOOP_CYCLE_COUNT = 10; private static final double STROBE_FAST_DURATION = 0.1; private static final double STROBE_SLOW_DURATION = 0.2; private static final double BREATH_DURATION = 1.0; - private static final double RAINBOW_CYCLE_LENGTH = 25.0; - private static final double RAINBOW_DURATION = 0.25; + private static final double PULSE_DURATION = 0.5; + private static final double RAINBOW_CYCLE_LENGTH = 30.0; + private static final double RAINBOW_DURATION = .25; private static final double WAVE_EXPONENT = 0.4; private static final double WAVE_FAST_CYCLE_LENGTH = 25.0; private static final double WAVE_FAST_DURATION = 0.25; + private static final double WAVE_MEDIUM_DURATION = 0.75; private static final double WAVE_SLOW_CYCLE_LENGTH = 25.0; private static final double WAVE_SLOW_DURATION = 3.0; private static final double WAVE_ALLIANCE_CYCLE_LENGTH = 15.0; @@ -89,10 +90,7 @@ protected LEDs() { () -> { synchronized (this) { breath( - Section.STATIC_LOW, - Color.kWhite, - Color.kBlack, - System.currentTimeMillis() / 1000.0); + Section.FULL, Color.kWhite, Color.kBlack, System.currentTimeMillis() / 1000.0); this.updateLEDs(); } }); @@ -130,14 +128,14 @@ private void updateLEDPattern() { // Auto fade solid(1.0 - ((Timer.getFPGATimestamp() - lastEnabledTime) / AUTO_FADE_TIME), Color.kGreen); - } else if (lowBatteryAlert) { - // Low battery - solid(Section.FULL, Color.kOrangeRed); - - } else if (PRIDE_LEDS) { + } else if (Constants.DEMO_MODE) { // Pride stripes updateToPridePattern(); + } else if (lowBatteryAlert) { + // Low battery + solid(Section.FULL, new Color(255, 20, 0)); + } else { // Default pattern updateToDisabledPattern(); @@ -158,38 +156,33 @@ private void updateToTeleopPattern() { // Set special modes // Demo mode background - if (demoMode) { + if (Constants.DEMO_MODE) { wave( Section.FULL, - Color.kDarkOrange, + new Color(255, 30, 0), Color.kDarkBlue, WAVE_SLOW_CYCLE_LENGTH, - WAVE_SLOW_DURATION); + WAVE_MEDIUM_DURATION); } - if (distraction) { - strobe(Section.SHOULDER, Color.kWhite, STROBE_FAST_DURATION); - } else if (endgameAlert) { - strobe(Section.SHOULDER, Color.kBlue, STROBE_SLOW_DURATION); + if (endgameAlert) { + // Endgame alert + strobe(Section.FULL, Color.kYellow, STROBE_SLOW_DURATION); } } private void updateToAutoPattern() { - wave( - Section.FULL, - Color.kDarkOrange, - Color.kDarkBlue, - WAVE_FAST_CYCLE_LENGTH, - WAVE_FAST_DURATION); - if (autoFinished) { - double fullTime = LENGTH / WAVE_FAST_CYCLE_LENGTH * WAVE_FAST_DURATION; - solid((Timer.getFPGATimestamp() - autoFinishedTime) / fullTime, Color.kGreen); - } + orangePulse(Section.FULL, PULSE_DURATION); + + // if (autoFinished) { + // double fullTime = LENGTH / WAVE_FAST_CYCLE_LENGTH * WAVE_FAST_DURATION; + // solid((Timer.getFPGATimestamp() - autoFinishedTime) / fullTime, Color.kGreen); + // } } private void updateToDisabledPattern() { if (assignedAlliance) { - if (alliance == Alliance.Red) { + if (Field2d.getInstance().getAlliance() == Alliance.Red) { wave( Section.FULL, Color.kRed, @@ -207,7 +200,7 @@ private void updateToDisabledPattern() { } else { wave( Section.FULL, - Color.kDarkOrange, + new Color(255, 30, 0), Color.kDarkBlue, WAVE_SLOW_CYCLE_LENGTH, WAVE_SLOW_DURATION); @@ -233,7 +226,7 @@ private void updateToPridePattern() { new Color(0.15, 0.3, 1.0)), 3, 5.0); - switch (alliance) { + switch (Field2d.getInstance().getAlliance()) { case Red: solid(Section.STATIC_LOW, Color.kRed); break; @@ -246,9 +239,8 @@ private void updateToPridePattern() { } private void updateState() { - // Update alliance color + // check for alliance assignment when connected to FMS if (DriverStation.isFMSAttached()) { - alliance = DriverStation.getAlliance().orElse(Alliance.Blue); assignedAlliance = true; } else { assignedAlliance = false; @@ -268,10 +260,6 @@ private void updateState() { } } - public void setDistraction(boolean distraction) { - this.distraction = distraction; - } - public void setFallen(boolean fallen) { this.fallen = fallen; } @@ -291,10 +279,6 @@ public void setLowBatteryAlert(boolean lowBatteryAlert) { this.lowBatteryAlert = lowBatteryAlert; } - public void setDemoMode(boolean demoMode) { - this.demoMode = demoMode; - } - protected abstract void updateLEDs(); protected abstract void setLEDBuffer(int index, Color color); @@ -366,6 +350,65 @@ private void wave(Section section, Color c1, Color c2, double cycleLength, doubl } } + private void fire(Section section, double duration) { + double x = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; + double[] heat = new double[section.end() - section.start()]; + double xDiffPerLed = (2.0 * Math.PI) / heat.length; + + for (int i = 0; i < heat.length; i++) { + x += xDiffPerLed; + heat[i] = (Math.sin(x) + 1.0) / 2.0; // Heat level between 0 and 1 + } + + for (int i = 0; i < heat.length; i++) { + double ratio = heat[i]; + // Use shades of blue and orange for the fire effect + int red = (int) (255 * ratio); + int green = (int) (20 * ratio); + int blue = 0; // Blend blue and orange + + // Simulate rising and falling effect + double sinValue = Math.sin(x + (i * 0.2)); + int offset = (int) ((sinValue + 1) / 2 * 255); // Scale to 0-255 + + // Apply the color and intensity to the LED + setLEDBuffer( + section.start() + i, + new Color( + Math.max(0, red - offset), Math.max(0, green - offset), Math.max(0, blue - offset))); + } + } + + private void orangePulse(Section section, double duration) { + double x = (1 - ((Timer.getFPGATimestamp() % duration) / duration)) * 2.0 * Math.PI; + double[] heat = new double[section.end() - section.start()]; + double xDiffPerLed = (2.0 * Math.PI) / heat.length; + + for (int i = 0; i < heat.length; i++) { + x += xDiffPerLed; + heat[i] = (Math.sin(x) + 1.0) / 2.0; // Heat level between 0 and 1 + } + + for (int i = 0; i < heat.length; i++) { + double ratio = heat[i]; + + // Orange color + int red = (int) (255 * ratio); + int green = (int) (30 * ratio); + int blue = 0; + // int blue = (int) (10 * ratio); + + // Simulate rising and falling effect + int offset = (int) (2 * Math.sin(x + (i * 0.2))); + + // Apply the color to the LED + setLEDBuffer( + section.start() + i, + new Color( + Math.max(0, red - offset), Math.max(0, green - offset), Math.max(0, blue - offset))); + } + } + private void stripes(Section section, List colors, int length, double duration) { int offset = (int) (Timer.getFPGATimestamp() % duration / duration * length * colors.size()); for (int i = section.start(); i < section.end(); i++) { diff --git a/src/main/java/frc/lib/team3061/leds/LEDsRIO.java b/src/main/java/frc/lib/team3061/leds/LEDsRIO.java index 4694a9b7..1c9ba99a 100644 --- a/src/main/java/frc/lib/team3061/leds/LEDsRIO.java +++ b/src/main/java/frc/lib/team3061/leds/LEDsRIO.java @@ -8,10 +8,16 @@ public class LEDsRIO extends LEDs { private final AddressableLED leds; private final AddressableLEDBuffer buffer; + private boolean isGRB; + private boolean competitionBrightness; protected LEDsRIO() { leds = new AddressableLED(0); buffer = new AddressableLEDBuffer(ACTUAL_LENGTH); + // leds.setBitTiming(500, 200, 1200, 1300); + isGRB = true; + competitionBrightness = true; + leds.setLength(ACTUAL_LENGTH); leds.setData(buffer); leds.start(); @@ -24,9 +30,67 @@ protected void updateLEDs() { @Override protected void setLEDBuffer(int index, Color color) { - buffer.setLED(index, color); + int h = 0; + int s = 0; + int v = 0; + if (isGRB) { + color = changeToGRB(color); + int[] hsv = + RGBToHSV((int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255)); + h = hsv[0]; + s = hsv[1]; + v = hsv[2]; + } + + if (competitionBrightness) { + buffer.setLED(index, color); + } else { + buffer.setHSV(index, h, s, v); + } + if (MIRROR_LEDS) { - buffer.setLED(ACTUAL_LENGTH - index - 1, color); + if (competitionBrightness) { + buffer.setLED(ACTUAL_LENGTH - index - 1, color); + } else { + buffer.setHSV(ACTUAL_LENGTH - index - 1, h, s, v); + } } } + + static Color changeToGRB(Color color) { + return new Color(color.green, color.red, color.blue); + } + + static int[] RGBToHSV(int r, int g, int b) { + double red = r / 255.0; + double green = g / 255.0; + double blue = b / 255.0; + + double cMax = Math.max(red, Math.max(green, blue)); + double cMin = Math.min(red, Math.min(green, blue)); + + double delta = cMax - cMin; + + // Hue + int hue; + + if (delta == 0) { + hue = 0; + } else if (cMax == red) { + hue = (int) Math.round(60 * (((green - blue) / delta) % 6)); + } else if (cMax == green) { + hue = (int) Math.round(60 * (((blue - red) / delta) + 2)); + } else { + hue = (int) Math.round(60 * (((red - green) / delta) + 4)); + } + + // Saturation + double saturation = (cMax == 0) ? 0 : delta / cMax; + + // Convert final values to correct range + + return new int[] { + hue / 2, (int) Math.round(saturation * 255 / 2), (int) Math.round(cMax * 255) + }; + } } diff --git a/src/main/java/frc/lib/team3061/vision/Vision.java b/src/main/java/frc/lib/team3061/vision/Vision.java index e3441c6e..5a388cb9 100644 --- a/src/main/java/frc/lib/team3061/vision/Vision.java +++ b/src/main/java/frc/lib/team3061/vision/Vision.java @@ -38,6 +38,7 @@ public class Vision extends SubsystemBase { private double[] lastTimestamps; private final Pose2d[] detectedAprilTags; private int[] cyclesWithNoResults; + private int[] updatePoseCount; private AprilTagFieldLayout layout; private Alert noAprilTagLayoutAlert = @@ -72,6 +73,7 @@ public Vision(VisionIO[] visionIOs) { this.visionIOs = visionIOs; this.lastTimestamps = new double[visionIOs.length]; this.cyclesWithNoResults = new int[visionIOs.length]; + this.updatePoseCount = new int[visionIOs.length]; this.ios = new VisionIOInputsAutoLogged[visionIOs.length]; for (int i = 0; i < visionIOs.length; i++) { this.ios[i] = new VisionIOInputsAutoLogged(); @@ -145,16 +147,18 @@ public void periodic() { private void processNewVisionData(int i) { // only process the vision data if the timestamp is newer than the last one - if (this.lastTimestamps[i] < ios[i].lastCameraTimestamp) { - this.lastTimestamps[i] = ios[i].lastCameraTimestamp; + if (this.lastTimestamps[i] < ios[i].estimatedCameraPoseTimestamp) { + this.lastTimestamps[i] = ios[i].estimatedCameraPoseTimestamp; Pose3d estimatedRobotPose3d = ios[i].estimatedCameraPose.plus( RobotConfig.getInstance().getRobotToCameraTransforms()[i].inverse()); Pose2d estimatedRobotPose2d = estimatedRobotPose3d.toPose2d(); - // only update the pose estimator if the vision subsystem is enabled and vision's estimated + // only update the pose estimator if the vision subsystem is enabled, the estimated pose is in + // the past, the ambiguity is less than the threshold, and vision's estimated // pose is within the specified tolerance of the current pose if (isEnabled + && ios[i].ambiguity < AMBIGUITY_THRESHOLD && estimatedRobotPose2d .getTranslation() .getDistance(odometry.getEstimatedPosition().getTranslation()) @@ -162,10 +166,16 @@ private void processNewVisionData(int i) { // when updating the pose estimator, specify standard deviations based on the distance // from the robot to the AprilTag (the greater the distance, the less confident we are // in the measurement) - Matrix stdDev = getStandardDeviations(i, estimatedRobotPose2d, ios[i].minAmbiguity); - odometry.addVisionMeasurement( - estimatedRobotPose2d, ios[i].estimatedCameraPoseTimestamp, stdDev); + double timeStamp = + Math.min(ios[i].estimatedCameraPoseTimestamp, Logger.getRealTimestamp() / 1e6); + Matrix stdDev = getStandardDeviations(i, estimatedRobotPose2d, ios[i].ambiguity); + odometry.addVisionMeasurement(estimatedRobotPose2d, timeStamp, stdDev); isVisionUpdating = true; + this.updatePoseCount[i]++; + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/UpdatePoseCount", this.updatePoseCount[i]); + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/StdDevX", stdDev.get(0, 0)); + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/StdDevY", stdDev.get(1, 0)); + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/StdDevT", stdDev.get(2, 0)); } Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/CameraPose3d", ios[i].estimatedCameraPose); @@ -173,6 +183,10 @@ private void processNewVisionData(int i) { SUBSYSTEM_NAME + "/" + i + "/CameraPose2d", ios[i].estimatedCameraPose.toPose2d()); Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/RobotPose3d", estimatedRobotPose3d); Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/RobotPose2d", estimatedRobotPose2d); + Logger.recordOutput( + SUBSYSTEM_NAME + "/" + i + "/TimestampDifference", + Logger.getRealTimestamp() / 1e6 - ios[i].estimatedCameraPoseTimestamp); + this.cyclesWithNoResults[i] = 0; } else { this.cyclesWithNoResults[i] += 1; @@ -181,7 +195,8 @@ private void processNewVisionData(int i) { // if no tags have been seen for the specified number of cycles, "zero" the robot pose // such that old data is not seen in AdvantageScope if (cyclesWithNoResults[i] == EXPIRATION_COUNT) { - Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/RobotPose", new Pose2d()); + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/RobotPose2d", new Pose2d()); + Logger.recordOutput(SUBSYSTEM_NAME + "/" + i + "/RobotPose3d", new Pose3d()); } } @@ -204,12 +219,21 @@ public Pose3d getBestRobotPose() { Pose3d robotPoseFromMostRecentData = null; double mostRecentTimestamp = 0.0; for (int i = 0; i < visionIOs.length; i++) { - if (ios[i].estimatedCameraPoseTimestamp > mostRecentTimestamp) { - robotPoseFromMostRecentData = ios[i].estimatedCameraPose; + if (ios[i].estimatedCameraPoseTimestamp > mostRecentTimestamp + && ios[i].ambiguity < AMBIGUITY_THRESHOLD) { + robotPoseFromMostRecentData = + ios[i].estimatedCameraPose.plus( + RobotConfig.getInstance().getRobotToCameraTransforms()[i].inverse()); mostRecentTimestamp = ios[i].estimatedCameraPoseTimestamp; } } - return robotPoseFromMostRecentData; + + // if the most recent vision data is more than a half second old, don't return the robot pose + if (Math.abs(mostRecentTimestamp - Logger.getRealTimestamp() / 1e6) > 0.5) { + return null; + } else { + return robotPoseFromMostRecentData; + } } /** @@ -281,7 +305,8 @@ private Matrix getStandardDeviations( } // Adjust standard deviations based on the ambiguity of the pose - estStdDevs = estStdDevs.times(stdDevFactorAmbiguity.get() * minAmbiguity / MAXIMUM_AMBIGUITY); + estStdDevs = + estStdDevs.times(stdDevFactorAmbiguity.get() * minAmbiguity / AMBIGUITY_SCALE_FACTOR); return estStdDevs; } diff --git a/src/main/java/frc/lib/team3061/vision/VisionConstants.java b/src/main/java/frc/lib/team3061/vision/VisionConstants.java index b73d7c2a..79db729b 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionConstants.java +++ b/src/main/java/frc/lib/team3061/vision/VisionConstants.java @@ -17,12 +17,16 @@ private VisionConstants() { public static final String SUBSYSTEM_NAME = "Vision"; // the pose ambiguity must be less than this value for the target to be considered valid - public static final double MAXIMUM_AMBIGUITY = 0.2; + public static final double AMBIGUITY_THRESHOLD = 0.5; + + // this factor is applied to the pose ambiguity when calculating the standard deviation to pass to + // the pose estimator + public static final double AMBIGUITY_SCALE_FACTOR = 0.2; // the maximum distance between the robot's pose derived from the target and the current robot's // estimated pose for the target to be used to update the robot's pose (essentially, always use a // valid target to update the robot's pose) - public static final double MAX_POSE_DIFFERENCE_METERS = 2.0; + public static final double MAX_POSE_DIFFERENCE_METERS = 2.5; // the maximum distance between the robot and the target, for the target to be used to update the // robot's pose diff --git a/src/main/java/frc/lib/team3061/vision/VisionIO.java b/src/main/java/frc/lib/team3061/vision/VisionIO.java index 1e9ea5cd..71f5388b 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIO.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIO.java @@ -16,9 +16,10 @@ public interface VisionIO { public static class VisionIOInputs { Pose3d estimatedCameraPose = new Pose3d(); double estimatedCameraPoseTimestamp = 0.0; + double latencySecs = 0.0; boolean[] tagsSeen = new boolean[] {}; - double lastCameraTimestamp = 0.0; - double minAmbiguity = 0.0; + double ambiguity = 0.0; + boolean poseFromMultiTag = false; } /** diff --git a/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java b/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java index b007baaf..95b79da6 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java @@ -1,7 +1,5 @@ package frc.lib.team3061.vision; -import static frc.lib.team3061.vision.VisionConstants.*; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Transform3d; import frc.lib.team6328.util.Alert; @@ -11,7 +9,6 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonTrackedTarget; /** * PhotonVision-based implementation of the VisionIO interface. @@ -27,7 +24,6 @@ public class VisionIOPhotonVision implements VisionIO { private final PhotonCamera camera; private final PhotonPoseEstimator photonEstimator; private final boolean[] tagsSeen; - private double lastTimestamp = 0; private int cyclesWithNoResults = 0; /** @@ -53,36 +49,35 @@ public VisionIOPhotonVision(String cameraName, AprilTagFieldLayout layout) { @Override public void updateInputs(VisionIOInputs inputs) { Optional visionEstimate = this.photonEstimator.update(); - double latestTimestamp = camera.getLatestResult().getTimestampSeconds(); this.cyclesWithNoResults += 1; - boolean newResult = Math.abs(latestTimestamp - this.lastTimestamp) > 1e-5; - if (newResult) { - double minAmbiguity = 10.0; - for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { - if (target.getPoseAmbiguity() < minAmbiguity) { - minAmbiguity = target.getPoseAmbiguity(); - } - } - inputs.minAmbiguity = minAmbiguity; + visionEstimate.ifPresent( + estimate -> { + inputs.estimatedCameraPose = estimate.estimatedPose; + inputs.estimatedCameraPoseTimestamp = estimate.timestampSeconds; + inputs.latencySecs = camera.getLatestResult().getLatencyMillis() / 1000.0; + for (int i = 0; i < this.tagsSeen.length; i++) { + this.tagsSeen[i] = false; + } + for (int i = 0; i < estimate.targetsUsed.size(); i++) { + this.tagsSeen[estimate.targetsUsed.get(i).getFiducialId()] = true; + } + inputs.tagsSeen = this.tagsSeen; + inputs.poseFromMultiTag = estimate.strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; - visionEstimate.ifPresent( - estimate -> { - inputs.estimatedCameraPose = estimate.estimatedPose; - inputs.estimatedCameraPoseTimestamp = estimate.timestampSeconds; - for (int i = 0; i < this.tagsSeen.length; i++) { - this.tagsSeen[i] = false; - } - for (int i = 0; i < estimate.targetsUsed.size(); i++) { - this.tagsSeen[estimate.targetsUsed.get(i).getFiducialId()] = true; - } - inputs.tagsSeen = this.tagsSeen; - inputs.lastCameraTimestamp = latestTimestamp; - this.lastTimestamp = latestTimestamp; - this.cyclesWithNoResults = 0; - }); - } + inputs.ambiguity = 0; + for (int i = 0; i < estimate.targetsUsed.size(); i++) { + inputs.ambiguity += estimate.targetsUsed.get(i).getPoseAmbiguity(); + } + inputs.ambiguity /= estimate.targetsUsed.size(); + + if (inputs.poseFromMultiTag) { + inputs.ambiguity = 0.2; + } + + this.cyclesWithNoResults = 0; + }); // if no tags have been seen for the specified number of cycles, clear the array if (this.cyclesWithNoResults == EXPIRATION_COUNT) { diff --git a/src/main/java/frc/lib/team3061/vision/VisionIOSim.java b/src/main/java/frc/lib/team3061/vision/VisionIOSim.java index c792abfd..8b6d4900 100644 --- a/src/main/java/frc/lib/team3061/vision/VisionIOSim.java +++ b/src/main/java/frc/lib/team3061/vision/VisionIOSim.java @@ -30,7 +30,6 @@ public class VisionIOSim implements VisionIO { private final PhotonCamera camera = new PhotonCamera(CAMERA_NAME); private final PhotonPoseEstimator photonEstimator; private final boolean[] tagsSeen; - private double lastTimestamp = 0; private int cyclesWithNoResults = 0; private Supplier poseSupplier; @@ -48,7 +47,7 @@ public VisionIOSim( AprilTagFieldLayout layout, Supplier poseSupplier, Transform3d robotToCamera) { this.photonEstimator = new PhotonPoseEstimator( - layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, robotToCamera); + layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, robotToCamera.inverse()); this.poseSupplier = poseSupplier; this.visionSim = new VisionSystemSim(CAMERA_NAME); @@ -62,7 +61,7 @@ public VisionIOSim( this.cameraSim = new PhotonCameraSim(camera, cameraProp); - visionSim.addCamera(cameraSim, robotToCamera); + visionSim.addCamera(cameraSim, new Transform3d()); cameraSim.enableDrawWireframe(true); // the index of the array corresponds to the tag ID; so, add one since there is no tag ID 0 @@ -75,32 +74,38 @@ public VisionIOSim( * @param inputs the VisionIOInputs object to update with the latest data from the camera */ @Override - public synchronized void updateInputs(VisionIOInputs inputs) { + public void updateInputs(VisionIOInputs inputs) { this.visionSim.update(poseSupplier.get()); Optional visionEstimate = this.photonEstimator.update(); - double latestTimestamp = camera.getLatestResult().getTimestampSeconds(); this.cyclesWithNoResults += 1; - boolean newResult = Math.abs(latestTimestamp - this.lastTimestamp) > 1e-5; - if (newResult) { - visionEstimate.ifPresent( - estimate -> { - inputs.estimatedCameraPose = estimate.estimatedPose; - inputs.estimatedCameraPoseTimestamp = estimate.timestampSeconds; - for (int i = 0; i < this.tagsSeen.length; i++) { - this.tagsSeen[i] = false; - } - for (int i = 0; i < estimate.targetsUsed.size(); i++) { - this.tagsSeen[estimate.targetsUsed.get(i).getFiducialId()] = true; - } - inputs.tagsSeen = this.tagsSeen; - inputs.lastCameraTimestamp = latestTimestamp; - this.lastTimestamp = latestTimestamp; - this.cyclesWithNoResults = 0; - }); - } + visionEstimate.ifPresent( + estimate -> { + inputs.estimatedCameraPose = estimate.estimatedPose; + inputs.estimatedCameraPoseTimestamp = estimate.timestampSeconds; + for (int i = 0; i < this.tagsSeen.length; i++) { + this.tagsSeen[i] = false; + } + for (int i = 0; i < estimate.targetsUsed.size(); i++) { + this.tagsSeen[estimate.targetsUsed.get(i).getFiducialId()] = true; + } + inputs.tagsSeen = this.tagsSeen; + inputs.poseFromMultiTag = estimate.strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; + + inputs.ambiguity = 0; + for (int i = 0; i < estimate.targetsUsed.size(); i++) { + inputs.ambiguity += estimate.targetsUsed.get(i).getPoseAmbiguity(); + } + inputs.ambiguity /= estimate.targetsUsed.size(); + + if (inputs.poseFromMultiTag) { + inputs.ambiguity = 0.2; + } + + this.cyclesWithNoResults = 0; + }); // if no tags have been seen for the specified number of cycles, clear the array if (this.cyclesWithNoResults == EXPIRATION_COUNT) { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a46069fe..19d17e76 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,21 +27,21 @@ public final class Constants { // set to true in order to change all Tunable values via Shuffleboard public static final boolean TUNING_MODE = false; + public static final boolean DEMO_MODE = false; - private static final RobotType ROBOT = RobotType.ROBOT_PRACTICE; + private static final RobotType ROBOT = RobotType.ROBOT_SIMBOT_CTRE; private static final Alert invalidRobotAlert = new Alert("Invalid robot selected, using competition robot as default.", AlertType.ERROR); // FIXME: update for various robots public enum RobotType { - ROBOT_2023_NOVA_CTRE, - ROBOT_2023_NOVA_CTRE_FOC, - ROBOT_2023_NOVA, ROBOT_DEFAULT, ROBOT_SIMBOT, ROBOT_SIMBOT_CTRE, - ROBOT_PRACTICE + ROBOT_PRACTICE, + ROBOT_2024_ARTEMIS, + ROBOT_PRACTICE_BOARD } // FIXME: update for various robots @@ -50,7 +50,7 @@ public static RobotType getRobot() { if (ROBOT == RobotType.ROBOT_SIMBOT || ROBOT == RobotType.ROBOT_SIMBOT_CTRE) { // Invalid robot selected invalidRobotAlert.set(true); - return RobotType.ROBOT_DEFAULT; + return RobotType.ROBOT_2024_ARTEMIS; } else { return ROBOT; } @@ -63,10 +63,9 @@ public static RobotType getRobot() { public static Mode getMode() { switch (getRobot()) { case ROBOT_DEFAULT: - case ROBOT_2023_NOVA_CTRE: - case ROBOT_2023_NOVA_CTRE_FOC: - case ROBOT_2023_NOVA: case ROBOT_PRACTICE: + case ROBOT_PRACTICE_BOARD: + case ROBOT_2024_ARTEMIS: return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case ROBOT_SIMBOT: diff --git a/src/main/java/frc/robot/Field2d.java b/src/main/java/frc/robot/Field2d.java index f21629d6..eecb7081 100644 --- a/src/main/java/frc/robot/Field2d.java +++ b/src/main/java/frc/robot/Field2d.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.lib.team3061.RobotConfig; import frc.lib.team3061.drivetrain.Drivetrain; +import frc.lib.team3061.util.RobotOdometry; import frc.lib.team6328.util.FieldConstants; import java.util.ArrayList; import java.util.Arrays; @@ -32,7 +33,7 @@ public class Field2d { private Region2d[] regions; - private Alliance alliance; + private Alliance alliance = DriverStation.Alliance.Blue; /** * Get the singleton instance of the Field2d class. @@ -212,19 +213,15 @@ public Alliance getAlliance() { return alliance; } - public Pose2d getAllianceSpeakerCenter() { + public boolean hasFullyLeftAllianceSideOfField() { if (alliance == Alliance.Blue) { - return FieldConstants.BlueSpeaker.blueCenterSpeakerOpening; + return RobotOdometry.getInstance().getEstimatedPosition().getX() + > FieldConstants.StagingLocations.centerlineX + + RobotConfig.getInstance().getRobotLengthWithBumpers() / 2; } else { - return FieldConstants.RedSpeaker.redCenterSpeakerOpening; - } - } - - public Pose2d getOpponentSpeakerCenter() { - if (alliance == Alliance.Blue) { - return FieldConstants.RedSpeaker.redCenterSpeakerOpening; - } else { - return FieldConstants.BlueSpeaker.blueCenterSpeakerOpening; + return RobotOdometry.getInstance().getEstimatedPosition().getX() + < FieldConstants.StagingLocations.centerlineX + - RobotConfig.getInstance().getRobotLengthWithBumpers() / 2; } } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ff740092..d7a92955 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,7 @@ package frc.robot; -import com.ctre.phoenix6.SignalLogger; +import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.hal.AllianceStationID; @@ -100,8 +100,6 @@ public void robotInit() { LoggedPowerDistribution.getInstance(); - SignalLogger.setPath("/media/sda1"); - SignalLogger.start(); break; case SIM: @@ -164,6 +162,12 @@ public void robotInit() { PathPlannerLogging.setLogActivePathCallback( poses -> Logger.recordOutput("PathFollowing/activePath", poses.toArray(new Pose2d[0]))); + // Due to the nature of how Java works, the first run of a path following command could have a + // significantly higher delay compared with subsequent runs, as all the classes involved will + // need to be loaded. To help alleviate this issue, you can run a warmup command in the + // background when code starts. + FollowPathCommand.warmupCommand().schedule(); + // Start timers disabledTimer.reset(); disabledTimer.start(); @@ -218,6 +222,8 @@ public void robotPeriodic() { LEDs.getInstance().setAutoFinished(true); } + robotContainer.periodic(); + Threads.setCurrentThreadPriority(true, 10); } @@ -249,6 +255,8 @@ public void autonomousInit() { if (autonomousCommand != null) { autonomousCommand.schedule(); } + + robotContainer.autonomousInit(); } /** This method is invoked at the start of the teleop period. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a73d8be..18fea0cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -25,7 +24,6 @@ import frc.lib.team3061.drivetrain.DrivetrainIOGeneric; import frc.lib.team3061.drivetrain.swerve.SwerveModuleIO; import frc.lib.team3061.drivetrain.swerve.SwerveModuleIOTalonFXPhoenix6; -import frc.lib.team3061.gyro.GyroIO; import frc.lib.team3061.gyro.GyroIOPigeon2Phoenix6; import frc.lib.team3061.leds.LEDs; import frc.lib.team3061.pneumatics.Pneumatics; @@ -36,13 +34,12 @@ import frc.lib.team3061.vision.VisionIOPhotonVision; import frc.lib.team3061.vision.VisionIOSim; import frc.robot.Constants.Mode; -import frc.robot.commands.FeedForwardCharacterization; -import frc.robot.commands.FeedForwardCharacterization.FeedForwardCharacterizationData; import frc.robot.commands.TeleopSwerve; +import frc.robot.commands.WheelDiameterCharacterization; +import frc.robot.configs.ArtemisRobotConfig; import frc.robot.configs.DefaultRobotConfig; -import frc.robot.configs.NovaCTRERobotConfig; -import frc.robot.configs.NovaCTRETCFRobotConfig; -import frc.robot.configs.NovaRobotConfig; +import frc.robot.configs.GenericDrivetrainRobotConfig; +import frc.robot.configs.PracticeBoardConfig; import frc.robot.configs.PracticeRobotConfig; import frc.robot.operator_interface.OISelector; import frc.robot.operator_interface.OperatorInterface; @@ -64,7 +61,7 @@ public class RobotContainer { private OperatorInterface oi = new OperatorInterface() {}; private RobotConfig config; private Drivetrain drivetrain; - private Alliance lastAlliance = DriverStation.Alliance.Red; + private Alliance lastAlliance = Field2d.getInstance().getAlliance(); private Vision vision; private Subsystem subsystem; @@ -97,15 +94,18 @@ public RobotContainer() { if (Constants.getMode() != Mode.REPLAY) { switch (Constants.getRobot()) { - case ROBOT_2023_NOVA_CTRE: - case ROBOT_2023_NOVA_CTRE_FOC: + case ROBOT_PRACTICE_BOARD: + { + createPracticeBoardSubsystem(); + break; + } case ROBOT_PRACTICE: + case ROBOT_2024_ARTEMIS: { createCTRESubsystems(); break; } case ROBOT_DEFAULT: - case ROBOT_2023_NOVA: case ROBOT_SIMBOT: { createSubsystems(); @@ -114,7 +114,6 @@ public RobotContainer() { case ROBOT_SIMBOT_CTRE: { createCTRESimSubsystems(); - break; } default: @@ -152,26 +151,26 @@ private void createRobotConfig() { case ROBOT_DEFAULT: config = new DefaultRobotConfig(); break; - case ROBOT_2023_NOVA_CTRE: - case ROBOT_SIMBOT_CTRE: - config = new NovaCTRERobotConfig(); - break; - case ROBOT_2023_NOVA_CTRE_FOC: - config = new NovaCTRETCFRobotConfig(); - break; - case ROBOT_2023_NOVA: case ROBOT_SIMBOT: - config = new NovaRobotConfig(); + config = new GenericDrivetrainRobotConfig(); break; case ROBOT_PRACTICE: config = new PracticeRobotConfig(); break; + case ROBOT_2024_ARTEMIS: + case ROBOT_SIMBOT_CTRE: + config = new ArtemisRobotConfig(); + break; + case ROBOT_PRACTICE_BOARD: + config = new PracticeBoardConfig(); + break; + default: + break; } } private void createCTRESubsystems() { - DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); - drivetrain = new Drivetrain(drivetrainIO); + drivetrain = new Drivetrain(new DrivetrainIOCTRE()); String[] cameraNames = config.getCameraNames(); VisionIO[] visionIOs = new VisionIO[cameraNames.length]; @@ -211,10 +210,14 @@ private void createSubsystems() { new SwerveModuleIOTalonFXPhoenix6( 3, driveMotorCANIDs[3], steerMotorCANDIDs[3], steerEncoderCANDIDs[3], steerOffsets[3]); - GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); - DrivetrainIO drivetrainIO = - new DrivetrainIOGeneric(gyro, flModule, frModule, blModule, brModule); - drivetrain = new Drivetrain(drivetrainIO); + drivetrain = + new Drivetrain( + new DrivetrainIOGeneric( + new GyroIOPigeon2Phoenix6(config.getGyroCANID()), + flModule, + frModule, + blModule, + brModule)); // FIXME: create the hardware-specific subsystem class subsystem = new Subsystem(new SubsystemIO() {}); @@ -224,20 +227,7 @@ private void createSubsystems() { } if (Constants.getRobot() == Constants.RobotType.ROBOT_SIMBOT) { - AprilTagFieldLayout layout; - try { - layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); - } catch (IOException e) { - layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); - } - vision = - new Vision( - new VisionIO[] { - new VisionIOSim( - layout, - drivetrain::getPose, - RobotConfig.getInstance().getRobotToCameraTransforms()[0]) - }); + vision = new Vision(new VisionIO[] {new VisionIO() {}}); } else { String[] cameraNames = config.getCameraNames(); VisionIO[] visionIOs = new VisionIO[cameraNames.length]; @@ -276,11 +266,15 @@ private void createCTRESimSubsystems() { // FIXME: create the hardware-specific subsystem class } + private void createPracticeBoardSubsystem() { + // change the following to connect the subsystem being tested to actual hardware + drivetrain = new Drivetrain(new DrivetrainIO() {}); + vision = new Vision(new VisionIO[] {new VisionIO() {}}); + } + /** * Creates the field from the defined regions and transition points from one region to its * neighbor. The field is used to generate paths. - * - *

FIXME: update for 2024 regions */ private void constructField() { Field2d.getInstance().setRegions(new Region2d[] {}); @@ -326,7 +320,7 @@ private void configureButtonBindings() { && DriverStation.getMatchTime() <= Math.round(endgameAlert1.get())) .onTrue( Commands.run(() -> LEDs.getInstance().setEndgameAlert(true)) - .withTimeout(1.5) + .withTimeout(1) .andThen( Commands.run(() -> LEDs.getInstance().setEndgameAlert(false)) .withTimeout(1.0))); @@ -338,9 +332,9 @@ private void configureButtonBindings() { .onTrue( Commands.sequence( Commands.run(() -> LEDs.getInstance().setEndgameAlert(true)).withTimeout(0.5), - Commands.run(() -> LEDs.getInstance().setEndgameAlert(false)).withTimeout(0.5), + Commands.run(() -> LEDs.getInstance().setEndgameAlert(false)).withTimeout(0.25), Commands.run(() -> LEDs.getInstance().setEndgameAlert(true)).withTimeout(0.5), - Commands.run(() -> LEDs.getInstance().setEndgameAlert(false)).withTimeout(1.0))); + Commands.run(() -> LEDs.getInstance().setEndgameAlert(false)).withTimeout(0.25))); // interrupt all commands by running a command that requires every subsystem. This is used to // recover to a known state if the robot becomes "stuck" in a command. @@ -362,6 +356,10 @@ private void configureAutoCommands() { NamedCommands.registerCommand( "disableXStance", Commands.runOnce(drivetrain::disableXstance, drivetrain)); NamedCommands.registerCommand("wait5Seconds", Commands.waitSeconds(5.0)); + NamedCommands.registerCommand( + "EnableRotationOverride", Commands.runOnce(drivetrain::enableRotationOverride)); + NamedCommands.registerCommand( + "DisableRotationOverride", Commands.runOnce(drivetrain::disableRotationOverride)); // build auto path commands @@ -398,36 +396,6 @@ private void configureAutoCommands() { drivetrain); autoChooser.addOption("Start Point", startPoint); - /************ Drive Characterization ************ - * - * useful for characterizing the swerve modules for driving (i.e, determining kS and kV) - * - */ - autoChooser.addOption( - "Swerve Drive Characterization", - new FeedForwardCharacterization( - drivetrain, - true, - new FeedForwardCharacterizationData("drive"), - drivetrain::runDriveCharacterizationVolts, - drivetrain::getDriveCharacterizationVelocity, - drivetrain::getDriveCharacterizationAcceleration)); - - /************ Swerve Rotate Characterization ************ - * - * useful for characterizing the swerve modules for rotating (i.e, determining kS and kV) - * - */ - autoChooser.addOption( - "Swerve Rotate Characterization", - new FeedForwardCharacterization( - drivetrain, - true, - new FeedForwardCharacterizationData("rotate"), - drivetrain::runRotateCharacterizationVolts, - drivetrain::getRotateCharacterizationVelocity, - drivetrain::getRotateCharacterizationAcceleration)); - /************ Distance Test ************ * * used for empirically determining the wheel diameter @@ -504,6 +472,23 @@ private void configureAutoCommands() { Commands.run( () -> drivetrain.drive(0.1, -0.1, 0.0, true, false), drivetrain))))); + /************ Drive Wheel Diameter Characterization ************ + * + * useful for characterizing the drive wheel diameter + * + */ + autoChooser.addOption( // start by driving slowing in a circle to align wheels + "Drive Wheel Diameter Characterization", + Commands.sequence( + Commands.deadline( + Commands.waitSeconds(0.5), + Commands.run(() -> drivetrain.drive(0.0, 0.0, 0.1, true, false), drivetrain)), + Commands.deadline( + Commands.waitSeconds(0.25), + Commands.run(() -> drivetrain.drive(0.0, 0.0, 0.0, true, false), drivetrain)), + new WheelDiameterCharacterization(drivetrain)) + .withName("Drive Wheel Diameter Characterization")); + Shuffleboard.getTab("MAIN").add(autoChooser.getSendableChooser()); } @@ -525,65 +510,71 @@ private void configureDrivetrainCommands() { oi.getLock180Button() .whileTrue( new TeleopSwerve( - drivetrain, - oi::getTranslateX, - oi::getTranslateY, - () -> - (drivetrain.getPose().getRotation().getDegrees() > -90 - && drivetrain.getPose().getRotation().getDegrees() < 90) - ? Rotation2d.fromDegrees(0.0) - : Rotation2d.fromDegrees(180.0))); - - oi.getLockToSpeakerButton() - .whileTrue( - new TeleopSwerve( - drivetrain, - oi::getTranslateX, - oi::getTranslateY, - () -> { - Transform2d translation = - new Transform2d( - Field2d.getInstance().getAllianceSpeakerCenter().getX() - - drivetrain.getPose().getX(), - Field2d.getInstance().getAllianceSpeakerCenter().getY() - - drivetrain.getPose().getY(), - new Rotation2d()); - return new Rotation2d(Math.atan2(translation.getY(), translation.getX())); - })); + drivetrain, + oi::getTranslateX, + oi::getTranslateY, + () -> + (drivetrain.getPose().getRotation().getDegrees() > -90 + && drivetrain.getPose().getRotation().getDegrees() < 90) + ? Rotation2d.fromDegrees(0.0) + : Rotation2d.fromDegrees(180.0)) + .withName("lock 180")); // field-relative toggle oi.getFieldRelativeButton() .toggleOnTrue( Commands.either( - Commands.runOnce(drivetrain::disableFieldRelative, drivetrain), - Commands.runOnce(drivetrain::enableFieldRelative, drivetrain), - drivetrain::getFieldRelative)); + Commands.runOnce(drivetrain::disableFieldRelative, drivetrain), + Commands.runOnce(drivetrain::enableFieldRelative, drivetrain), + drivetrain::getFieldRelative) + .withName("toggle field relative")); // slow-mode toggle oi.getTranslationSlowModeButton() - .onTrue(Commands.runOnce(drivetrain::enableTranslationSlowMode, drivetrain)); + .onTrue( + Commands.runOnce(drivetrain::enableTranslationSlowMode, drivetrain) + .withName("enable translation slow mode")); oi.getTranslationSlowModeButton() - .onFalse(Commands.runOnce(drivetrain::disableTranslationSlowMode, drivetrain)); + .onFalse( + Commands.runOnce(drivetrain::disableTranslationSlowMode, drivetrain) + .withName("disable translation slow mode")); oi.getRotationSlowModeButton() - .onTrue(Commands.runOnce(drivetrain::enableRotationSlowMode, drivetrain)); + .onTrue( + Commands.runOnce(drivetrain::enableRotationSlowMode, drivetrain) + .withName("enable rotation slow mode")); oi.getRotationSlowModeButton() - .onFalse(Commands.runOnce(drivetrain::disableRotationSlowMode, drivetrain)); + .onFalse( + Commands.runOnce(drivetrain::disableRotationSlowMode, drivetrain) + .withName("disable rotation slow mode")); // reset gyro to 0 degrees - oi.getResetGyroButton().onTrue(Commands.runOnce(drivetrain::zeroGyroscope, drivetrain)); + oi.getResetGyroButton() + .onTrue(Commands.runOnce(drivetrain::zeroGyroscope, drivetrain).withName("zero gyro")); // reset pose based on vision oi.getResetPoseToVisionButton() .onTrue( - Commands.runOnce(() -> drivetrain.resetPoseToVision(() -> vision.getBestRobotPose()))); + Commands.repeatingSequence(Commands.none()) + .until(() -> vision.getBestRobotPose() != null) + .andThen( + Commands.runOnce( + () -> drivetrain.resetPoseToVision(() -> vision.getBestRobotPose()))) + .ignoringDisable(true) + .withName("reset pose to vision")); // x-stance - oi.getXStanceButton().onTrue(Commands.runOnce(drivetrain::enableXstance, drivetrain)); - oi.getXStanceButton().onFalse(Commands.runOnce(drivetrain::disableXstance, drivetrain)); + oi.getXStanceButton() + .onTrue( + Commands.runOnce(drivetrain::enableXstance, drivetrain).withName("enable x-stance")); + oi.getXStanceButton() + .onFalse( + Commands.runOnce(drivetrain::disableXstance, drivetrain).withName("disable x-stance")); // turbo - oi.getTurboButton().onTrue(Commands.runOnce(drivetrain::enableTurbo, drivetrain)); - oi.getTurboButton().onFalse(Commands.runOnce(drivetrain::disableTurbo, drivetrain)); + oi.getTurboButton() + .onTrue(Commands.runOnce(drivetrain::enableTurbo, drivetrain).withName("enable turbo")); + oi.getTurboButton() + .onFalse(Commands.runOnce(drivetrain::disableTurbo, drivetrain).withName("disable turbo")); } private void configureSubsystemCommands() { @@ -592,12 +583,18 @@ private void configureSubsystemCommands() { private void configureVisionCommands() { // enable/disable vision - oi.getVisionIsEnabledSwitch().onTrue(Commands.runOnce(() -> vision.enable(true))); + oi.getVisionIsEnabledSwitch() + .onTrue( + Commands.runOnce(() -> vision.enable(true)) + .ignoringDisable(true) + .withName("enable vision")); oi.getVisionIsEnabledSwitch() .onFalse( Commands.parallel( - Commands.runOnce(() -> vision.enable(false), vision), - Commands.runOnce(drivetrain::resetPoseRotationToGyro))); + Commands.runOnce(() -> vision.enable(false), vision), + Commands.runOnce(drivetrain::resetPoseRotationToGyro)) + .ignoringDisable(true) + .withName("disable vision")); } /** @@ -621,4 +618,8 @@ public void checkAllianceColor() { Field2d.getInstance().updateAlliance(this.lastAlliance); } } + + public void periodic() {} + + public void autonomousInit() {} } diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java index 0a8bf36e..fb0d9148 100644 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -14,11 +14,13 @@ 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.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.lib.team3061.RobotConfig; import frc.lib.team3061.drivetrain.Drivetrain; import frc.lib.team6328.util.TunableNumber; +import frc.robot.Field2d; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -75,7 +77,7 @@ public class DriveToPose extends Command { private static final TunableNumber thetaTolerance = new TunableNumber( "DriveToPose/ThetaTolerance", RobotConfig.getInstance().getDriveToPoseThetaTolerance()); - private static final TunableNumber timeout = new TunableNumber("DriveToPose/timeout", 2.0); + private static final TunableNumber timeout = new TunableNumber("DriveToPose/timeout", 5.0); private final ProfiledPIDController xController = new ProfiledPIDController( @@ -197,7 +199,10 @@ public void execute() { if (yController.atGoal()) yVelocity = 0.0; if (thetaController.atGoal()) thetaVelocity = 0.0; - drivetrain.drive(xVelocity, yVelocity, thetaVelocity, true, true); + int allianceMultiplier = Field2d.getInstance().getAlliance() == Alliance.Blue ? 1 : -1; + + drivetrain.drive( + allianceMultiplier * xVelocity, allianceMultiplier * yVelocity, thetaVelocity, true, true); } /** diff --git a/src/main/java/frc/robot/commands/WheelDiameterCharacterization.java b/src/main/java/frc/robot/commands/WheelDiameterCharacterization.java new file mode 100644 index 00000000..1ecd5491 --- /dev/null +++ b/src/main/java/frc/robot/commands/WheelDiameterCharacterization.java @@ -0,0 +1,91 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.team3061.RobotConfig; +import frc.lib.team3061.drivetrain.Drivetrain; +import frc.lib.team3061.util.RobotOdometry; +import frc.lib.team6328.util.TunableNumber; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class WheelDiameterCharacterization extends Command { + private static final TunableNumber characterizationSpeed = + new TunableNumber("WheelDiameterCharacterization/SpeedRadsPerSec", 0.5); + private static final double DRIVE_RADIUS = + Math.hypot( + RobotConfig.getInstance().getTrackwidth() / 2.0, + RobotConfig.getInstance().getWheelbase() / 2.0); + private static final DoubleSupplier gyroYawRadsSupplier = + () -> RobotOdometry.getInstance().getEstimatedPosition().getRotation().getRadians(); + + private final Drivetrain drivetrain; + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0); + + private double lastGyroYawRads = 0.0; + private double accumulatedGyroYawRads = 0.0; + + private double[] startWheelPositions; + + private double currentEffectiveWheelRadius = 0.0; + + public WheelDiameterCharacterization(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + // Reset + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + accumulatedGyroYawRads = 0.0; + + startWheelPositions = drivetrain.getWheelDiameterCharacterizationPosition(); + + omegaLimiter.reset(0); + } + + @Override + public void execute() { + // Run drive at velocity + drivetrain.runWheelDiameterCharacterization( + omegaLimiter.calculate(characterizationSpeed.get())); + + // Get yaw and wheel positions + accumulatedGyroYawRads += + MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + double averageWheelPosition = 0.0; + double[] wheelPositions = drivetrain.getWheelDiameterCharacterizationPosition(); + for (int i = 0; i < 4; i++) { + averageWheelPosition += Math.abs(wheelPositions[i] - startWheelPositions[i]); + } + averageWheelPosition /= 4.0; + + currentEffectiveWheelRadius = (accumulatedGyroYawRads * DRIVE_RADIUS) / averageWheelPosition; + Logger.recordOutput("WheelDiameterCharacterization/DrivePosition", averageWheelPosition); + Logger.recordOutput( + "WheelDiameterCharacterization/AccumulatedGyroYawRads", accumulatedGyroYawRads); + Logger.recordOutput( + "WheelDiameterCharacterization/CurrentWheelDiameterMeters", + currentEffectiveWheelRadius * 2.0); + } + + @Override + public void end(boolean interrupted) { + if (accumulatedGyroYawRads <= Math.PI * 2.0) { + System.out.println("Not enough data for characterization"); + } else { + System.out.println( + "Effective Wheel Diameter: " + currentEffectiveWheelRadius * 2.0 + " meters"); + } + } +} diff --git a/src/main/java/frc/robot/configs/ArtemisRobotConfig.java b/src/main/java/frc/robot/configs/ArtemisRobotConfig.java new file mode 100644 index 00000000..051c9683 --- /dev/null +++ b/src/main/java/frc/robot/configs/ArtemisRobotConfig.java @@ -0,0 +1,474 @@ +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.drivetrain.swerve.SwerveConstants; + +/* + * 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 ArtemisRobotConfig extends RobotConfig { + private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 1; + private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 2; + private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 22; + private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = 0.358398; + + private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 3; + private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 4; + private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 23; + private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = 0.024414; + + private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7; + private static final int BACK_LEFT_MODULE_STEER_MOTOR = 8; + private static final int BACK_LEFT_MODULE_STEER_ENCODER = 24; + private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = -0.025635; + + private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 5; + private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 6; + private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 25; + private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = -0.491699; + + private static final int GYRO_ID = 26; + + private static final double TRACKWIDTH_METERS = 0.57785; // 22.75 + private static final double WHEELBASE_METERS = 0.57785; // 22.75 + private static final double WHEEL_DIAMETER_METERS = 0.1027125; + private static final double ROBOT_WIDTH_WITH_BUMPERS = + 0.88265; // meters //34.75 in , measure the actual bumpers + private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.88265; // meters // 34.75 in same above + + /* Angle Motor PID Values */ + private static final double ANGLE_KP = 100.0; + private static final double ANGLE_KI = 0.0; + private static final double ANGLE_KD = 0.05; + + private static final double ANGLE_KS = 0.24719; + private static final double ANGLE_KV = 2.5845; // rps + private static final double ANGLE_KA = 0.030892; + + /* Drive Motor PID Values */ + private static final double DRIVE_KP = 10.0; + private static final double DRIVE_KI = 0.0; + private static final double DRIVE_KD = 0.0; + + private static final double DRIVE_KS = 0.23819; + private static final double DRIVE_KV = 0.0; + private static final double DRIVE_KA = 0.0; + + private static final double MAX_VELOCITY_METERS_PER_SECOND = + 4.5; // FIXME: confirm max velocity with real robot + private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = + 0.05; // FIXME: Values taken from nova, need to be updated + private static final double SLOW_MODE_MULTIPLIER = + 0.75; // FIXME: Values taken from nova, need to be updated + + private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = + 9.467; // from Choreo estimate + private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = + 33.436; // from Choreo estimate + + private static final String CAN_BUS_NAME = "canbus1"; + + private static final String CAMERA_NAME_0 = "OV2311FR"; + private static final String CAMERA_NAME_1 = "OV2311BR"; + private static final String CAMERA_NAME_2 = "OV2311FL"; + private static final String CAMERA_NAME_3 = "OV2311BL"; + + // Front right camera + private static final Transform3d ROBOT_TO_CAMERA_0 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(11.064), + Units.inchesToMeters(-10.778), + Units.inchesToMeters(8.189)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(0))); + // pitch 45 degrees + + // Back right camera + private static final Transform3d ROBOT_TO_CAMERA_1 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-10.778), + Units.inchesToMeters(-11.064), + Units.inchesToMeters(8.189)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(-90))); + + // Front left camera + private static final Transform3d ROBOT_TO_CAMERA_2 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(10.778), + Units.inchesToMeters(11.064), + Units.inchesToMeters(8.189)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(90))); + + // Back left camera + private static final Transform3d ROBOT_TO_CAMERA_3 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(-11.064), + Units.inchesToMeters(10.778), + Units.inchesToMeters(8.189)), + new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(180))); + + private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 3.5; + private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 10; + private static final double AUTO_DRIVE_P_CONTROLLER = 5.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 = 5.0; + 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 = 4.5; + private static final double DRIVE_TO_POSE_THETA_KI = 0; + private static final double DRIVE_TO_POSE_THETA_KD = 0.0; + private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.06; + private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.02; + private static final double DRIVE_TO_POSE_MAX_VELOCITY = 1.25; + + private static final double SQUARING_SPEED_METERS_PER_SECOND = 1.0; + + // Drive Facing Angle constants + private static final double DRIVE_FACING_ANGLE_KP = 6.0; + private static final double DRIVE_FACING_ANGLE_KD = 0.1; + private static final double DRIVE_FACING_ANGLE_KI = 0.0; + + private static final int LED_COUNT = 35; + + @Override + public boolean getPhoenix6Licensed() { + return true; + } + + @Override + public double getSwerveAngleKP() { + return ANGLE_KP; + } + + @Override + public double getSwerveAngleKI() { + return ANGLE_KI; + } + + @Override + public double getSwerveAngleKD() { + return ANGLE_KD; + } + + @Override + public double getSwerveAngleKS() { + return ANGLE_KS; + } + + @Override + public double getSwerveAngleKV() { + return ANGLE_KV; + } + + @Override + public double getSwerveAngleKA() { + return ANGLE_KA; + } + + @Override + public double getSwerveDriveKP() { + return DRIVE_KP; + } + + @Override + public double getSwerveDriveKI() { + return DRIVE_KI; + } + + @Override + public double getSwerveDriveKD() { + return DRIVE_KD; + } + + @Override + public double getDriveKS() { + return DRIVE_KS; + } + + @Override + public double getDriveKV() { + return DRIVE_KV; + } + + @Override + public double getDriveKA() { + return DRIVE_KA; + } + + @Override + public SwerveConstants getSwerveConstants() { + return SwerveConstants.MK4I_L3_PLUS_CONSTANTS; + } + + @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_ROT, + FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, + BACK_LEFT_MODULE_STEER_OFFSET_ROT, + BACK_RIGHT_MODULE_STEER_OFFSET_ROT + }; + } + + @Override + public int getGyroCANID() { + return GYRO_ID; + } + + @Override + public double getTrackwidth() { + return TRACKWIDTH_METERS; + } + + @Override + public double getWheelbase() { + return WHEELBASE_METERS; + } + + @Override + public double getWheelDiameterMeters() { + return WHEEL_DIAMETER_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, ROBOT_TO_CAMERA_2, ROBOT_TO_CAMERA_3 + }; + } + + @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, CAMERA_NAME_2, CAMERA_NAME_3}; + } + + @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 DRIVE_TO_POSE_MAX_VELOCITY; + } + + @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; + } + + @Override + public double getDriveFacingAngleThetaKP() { + return DRIVE_FACING_ANGLE_KP; + } + + @Override + public double getDriveFacingAngleThetaKI() { + return DRIVE_FACING_ANGLE_KI; + } + + @Override + public double getDriveFacingAngleThetaKD() { + return DRIVE_FACING_ANGLE_KD; + } + + @Override + public double getOdometryUpdateFrequency() { + return 250.0; + } + + @Override + public LED_HARDWARE getLEDHardware() { + return LED_HARDWARE.RIO; + } + + @Override + public int getLEDCount() { + return LED_COUNT; + } + + @Override + public SWERVE_CONTROL_MODE getSwerveSteerControlMode() { + return SWERVE_CONTROL_MODE.VOLTAGE; + } + + @Override + public SWERVE_CONTROL_MODE getSwerveDriveControlMode() { + return SWERVE_CONTROL_MODE.TORQUE_CURRENT_FOC; + } +} diff --git a/src/main/java/frc/robot/configs/GenericDrivetrainRobotConfig.java b/src/main/java/frc/robot/configs/GenericDrivetrainRobotConfig.java new file mode 100644 index 00000000..eba08153 --- /dev/null +++ b/src/main/java/frc/robot/configs/GenericDrivetrainRobotConfig.java @@ -0,0 +1,430 @@ +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.drivetrain.swerve.SwerveConstants; + +/* + * 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 GenericDrivetrainRobotConfig 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_ROT = 0.27409; + + 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_ROT = -0.390381; + + 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_ROT = 0.827393; + + 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_ROT = -0.336670; + + 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 WHEEL_DIAMETER_METERS = 0.09845567409; + 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 = 100.0; + private static final double ANGLE_KI = 0.0; + private static final double ANGLE_KD = 0.05; + + private static final double ANGLE_KS = 0.1891233333; + private static final double ANGLE_KV = + 0.4399866667 * 2 * Math.PI; // convert from V/(radians/s) to V/(rotations/s) + private static final double ANGLE_KA = 0.001663333333; + + /* Drive Motor PID Values */ + private static final double DRIVE_KP = 0.005; + private static final double DRIVE_KI = 0.0; + private static final double DRIVE_KD = 0.0; + + private static final double DRIVE_KS = 0.4004375; + private static final double DRIVE_KV = 2.7637325; + private static final double DRIVE_KA = 0.0139575; + + private static final double MAX_VELOCITY_METERS_PER_SECOND = 3.5; + 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 = 11.365; + private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 36.0; + + private static final String CAN_BUS_NAME = "canbus1"; + + private static final String CAMERA_NAME_0 = "OV2311"; + + private static final String CAMERA_NAME_1 = "OV2311R"; + + 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 = 3.5; + private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 10; + private static final double AUTO_DRIVE_P_CONTROLLER = 5.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 = 5.0; + 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; + + // Drive Facing Angle constants + private static final double DRIVE_FACING_ANGLE_KP = 7.0; + private static final double DRIVE_FACING_ANGLE_KD = 0.1; + private static final double DRIVE_FACING_ANGLE_KI = 0.0; + + private static final int LED_COUNT = 85; + + @Override + public boolean getPhoenix6Licensed() { + return true; + } + + @Override + public double getSwerveAngleKP() { + return ANGLE_KP; + } + + @Override + public double getSwerveAngleKI() { + return ANGLE_KI; + } + + @Override + public double getSwerveAngleKD() { + return ANGLE_KD; + } + + @Override + public double getSwerveAngleKS() { + return ANGLE_KS; + } + + @Override + public double getSwerveAngleKV() { + return ANGLE_KV; + } + + @Override + public double getSwerveAngleKA() { + return ANGLE_KA; + } + + @Override + public double getSwerveDriveKP() { + return DRIVE_KP; + } + + @Override + public double getSwerveDriveKI() { + return DRIVE_KI; + } + + @Override + public double getSwerveDriveKD() { + return DRIVE_KD; + } + + @Override + public double getDriveKS() { + return DRIVE_KS; + } + + @Override + public double getDriveKV() { + return DRIVE_KV; + } + + @Override + public double getDriveKA() { + return DRIVE_KA; + } + + @Override + public SwerveConstants getSwerveConstants() { + return SwerveConstants.MK4I_L2_CONSTANTS; + } + + @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_ROT, + FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, + BACK_LEFT_MODULE_STEER_OFFSET_ROT, + BACK_RIGHT_MODULE_STEER_OFFSET_ROT + }; + } + + @Override + public int getGyroCANID() { + return GYRO_ID; + } + + @Override + public double getTrackwidth() { + return TRACKWIDTH_METERS; + } + + @Override + public double getWheelbase() { + return WHEELBASE_METERS; + } + + @Override + public double getWheelDiameterMeters() { + return WHEEL_DIAMETER_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; + } + + @Override + public double getDriveFacingAngleThetaKP() { + return DRIVE_FACING_ANGLE_KP; + } + + @Override + public double getDriveFacingAngleThetaKI() { + return DRIVE_FACING_ANGLE_KI; + } + + @Override + public double getDriveFacingAngleThetaKD() { + return DRIVE_FACING_ANGLE_KD; + } + + @Override + public LED_HARDWARE getLEDHardware() { + return LED_HARDWARE.RIO; + } + + @Override + public int getLEDCount() { + return LED_COUNT; + } +} diff --git a/src/main/java/frc/robot/configs/PracticeBoardConfig.java b/src/main/java/frc/robot/configs/PracticeBoardConfig.java new file mode 100644 index 00000000..92fb92cf --- /dev/null +++ b/src/main/java/frc/robot/configs/PracticeBoardConfig.java @@ -0,0 +1,448 @@ +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.drivetrain.swerve.SwerveConstants; + +/* + * 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 PracticeBoardConfig 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_ROT = -0.013428 + 0.5; + + 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_ROT = -0.342773; + + 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_ROT = 0.263184 + 0.5; + + 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_ROT = 0.117188; + + private static final int GYRO_ID = 3; + + private static final double TRACKWIDTH_METERS = 0.57785; // 22.75 + private static final double WHEELBASE_METERS = 0.57785; // 22.75 + private static final double WHEEL_DIAMETER_METERS = 0.09659072671; + private static final double ROBOT_WIDTH_WITH_BUMPERS = + 0.88265; // meters //34.75 in , measure the actual bumpers + private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.88265; // meters // 34.75 in same above + + /* Angle Motor PID Values */ + private static final double ANGLE_KP = 100.0; + private static final double ANGLE_KI = 0.0; + private static final double ANGLE_KD = 0.05; + + private static final double ANGLE_KS = 0.1891233333; + private static final double ANGLE_KV = + 0.4399866667 * 2 * Math.PI; // convert from V/(radians/s) to V/(rotations/s) + private static final double ANGLE_KA = 0.001663333333; + + /* Drive Motor PID Values */ + private static final double DRIVE_KP = 8.0; + private static final double DRIVE_KI = 0.0; + private static final double DRIVE_KD = 0.0; + + private static final double DRIVE_KS = 5.0; + private static final double DRIVE_KV = 0.0; + private static final double DRIVE_KA = 0.0; + + private static final double MAX_VELOCITY_METERS_PER_SECOND = 3.5; + 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 = 11.365; + private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 36.0; + + private static final String CAN_BUS_NAME = ""; + + private static final String CAMERA_NAME_0 = "OV2311L"; + private static final String CAMERA_NAME_1 = "OV2311F"; + + // left camera + private static final Transform3d ROBOT_TO_CAMERA_0 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(5.500), + Units.inchesToMeters(10.329), + Units.inchesToMeters(18.387)), + new Rotation3d(0, Units.degreesToRadians(-35), Units.degreesToRadians(90))); + // pitch 45 degrees + + // left camera + private static final Transform3d ROBOT_TO_CAMERA_1 = + new Transform3d( + new Translation3d( + Units.inchesToMeters(7.329), + Units.inchesToMeters(-8.500), + Units.inchesToMeters(18.387)), + new Rotation3d(0, Units.degreesToRadians(-35), Units.degreesToRadians(0))); + + private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 3.5; + private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 10; + private static final double AUTO_DRIVE_P_CONTROLLER = 5.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 = 5.0; + 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; + + // Drive Facing Angle constants + private static final double DRIVE_FACING_ANGLE_KP = 2.0; + private static final double DRIVE_FACING_ANGLE_KD = 0.1; + private static final double DRIVE_FACING_ANGLE_KI = 0.0; + + private static final int LED_COUNT = 85; + + @Override + public boolean getPhoenix6Licensed() { + return true; + } + + @Override + public double getSwerveAngleKP() { + return ANGLE_KP; + } + + @Override + public double getSwerveAngleKI() { + return ANGLE_KI; + } + + @Override + public double getSwerveAngleKD() { + return ANGLE_KD; + } + + @Override + public double getSwerveAngleKS() { + return ANGLE_KS; + } + + @Override + public double getSwerveAngleKV() { + return ANGLE_KV; + } + + @Override + public double getSwerveAngleKA() { + return ANGLE_KA; + } + + @Override + public double getSwerveDriveKP() { + return DRIVE_KP; + } + + @Override + public double getSwerveDriveKI() { + return DRIVE_KI; + } + + @Override + public double getSwerveDriveKD() { + return DRIVE_KD; + } + + @Override + public double getDriveKS() { + return DRIVE_KS; + } + + @Override + public double getDriveKV() { + return DRIVE_KV; + } + + @Override + public double getDriveKA() { + return DRIVE_KA; + } + + @Override + public SwerveConstants getSwerveConstants() { + return SwerveConstants.MK4I_L3_CONSTANTS; + } + + @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_ROT, + FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, + BACK_LEFT_MODULE_STEER_OFFSET_ROT, + BACK_RIGHT_MODULE_STEER_OFFSET_ROT + }; + } + + @Override + public int getGyroCANID() { + return GYRO_ID; + } + + @Override + public double getTrackwidth() { + return TRACKWIDTH_METERS; + } + + @Override + public double getWheelbase() { + return WHEELBASE_METERS; + } + + @Override + public double getWheelDiameterMeters() { + return WHEEL_DIAMETER_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; + } + + @Override + public double getDriveFacingAngleThetaKP() { + return DRIVE_FACING_ANGLE_KP; + } + + @Override + public double getDriveFacingAngleThetaKI() { + return DRIVE_FACING_ANGLE_KI; + } + + @Override + public double getDriveFacingAngleThetaKD() { + return DRIVE_FACING_ANGLE_KD; + } + + @Override + public double getOdometryUpdateFrequency() { + return 250.0; + } + + @Override + public LED_HARDWARE getLEDHardware() { + return LED_HARDWARE.RIO; + } + + @Override + public int getLEDCount() { + return LED_COUNT; + } + + @Override + public SWERVE_CONTROL_MODE getSwerveSteerControlMode() { + return SWERVE_CONTROL_MODE.VOLTAGE; + } + + @Override + public SWERVE_CONTROL_MODE getSwerveDriveControlMode() { + return SWERVE_CONTROL_MODE.TORQUE_CURRENT_FOC; + } +} diff --git a/src/main/java/frc/robot/configs/PracticeRobotConfig.java b/src/main/java/frc/robot/configs/PracticeRobotConfig.java index ac90053d..402cfb22 100644 --- a/src/main/java/frc/robot/configs/PracticeRobotConfig.java +++ b/src/main/java/frc/robot/configs/PracticeRobotConfig.java @@ -16,22 +16,22 @@ public class PracticeRobotConfig 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_ROT = -0.013428 + 0.5; + private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = 0.474121; 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_ROT = -0.342773; + private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = -0.358398; 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_ROT = 0.263184 + 0.5; + private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = -0.234863; 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_ROT = 0.117188; + private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = 0.161133; private static final int GYRO_ID = 3; @@ -68,7 +68,7 @@ public class PracticeRobotConfig extends RobotConfig { private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 11.365; private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 36.0; - private static final String CAN_BUS_NAME = "canbus1"; + private static final String CAN_BUS_NAME = ""; private static final String CAMERA_NAME_0 = "OV2311L"; private static final String CAMERA_NAME_1 = "OV2311F"; diff --git a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java index 409a99dc..ab0f3862 100644 --- a/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java +++ b/src/main/java/frc/robot/operator_interface/DualJoysticksOI.java @@ -59,11 +59,6 @@ public Trigger getLock180Button() { return new Trigger(() -> false); } - @Override - public Trigger getLockToSpeakerButton() { - return rotateJoystickButtons[2]; - } - @Override public Trigger getXStanceButton() { return rotateJoystickButtons[4]; diff --git a/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java index 6ff74e12..c90f3962 100644 --- a/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java +++ b/src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java @@ -62,11 +62,6 @@ public Trigger getLock180Button() { return translateJoystickButtons[2]; } - @Override - public Trigger getLockToSpeakerButton() { - return rotateJoystickButtons[3]; - } - @Override public Trigger getResetGyroButton() { return translateJoystickButtons[4]; diff --git a/src/main/java/frc/robot/operator_interface/OperatorInterface.java b/src/main/java/frc/robot/operator_interface/OperatorInterface.java index 835a1d57..26016eaa 100644 --- a/src/main/java/frc/robot/operator_interface/OperatorInterface.java +++ b/src/main/java/frc/robot/operator_interface/OperatorInterface.java @@ -13,6 +13,8 @@ /** Interface for all driver and operator controls. */ public interface OperatorInterface { + // drivetrain, generic + public default double getTranslateX() { return 0.0; } @@ -61,11 +63,10 @@ public default Trigger getTurboButton() { return new Trigger(() -> false); } - public default Trigger getInterruptAll() { - return new Trigger(() -> false); - } + // drivetrain, game-specific - public default Trigger getLockToSpeakerButton() { + // miscellaneous + public default Trigger getInterruptAll() { return new Trigger(() -> false); } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index f4612137..63dacbb5 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.1.0", + "version": "3.2.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.1.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.1.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.1.0" + "version": "3.2.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.1.0", + "version": "3.2.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..6dc648db 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d1720..03223850 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 86329e30..f85acd40 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.2", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.2" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.2", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 20d95984..0e80a16c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.5", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.5", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.5", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.5" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.5" + "version": "v2024.3.1" } ] } \ No newline at end of file