├── networktables.json ├── networktables.json.bck ├── gradle └── wrapper │ ├── gradle-wrapper.jar │ └── gradle-wrapper.properties ├── .wpilib └── wpilib_preferences.json ├── pull_latest_logs_from_rio.sh ├── .vscode ├── extensions.json ├── launch.json └── settings.json ├── src └── main │ ├── deploy │ ├── example.txt │ ├── pathplanner │ │ ├── autos │ │ │ ├── Start Point.auto │ │ │ ├── Differential Auto.auto │ │ │ ├── DistanceTestFast.auto │ │ │ ├── DistanceTestMed.auto │ │ │ ├── DistanceTestSlow.auto │ │ │ ├── RotationTestFast.auto │ │ │ ├── RotationTestSlow.auto │ │ │ ├── OvalTestFast.auto │ │ │ └── OvalTestSlow.auto │ │ ├── settings.json │ │ └── paths │ │ │ ├── Differential Path.path │ │ │ ├── Start Point.path │ │ │ ├── DistanceTestFast.path │ │ │ ├── DistanceTestMed.path │ │ │ ├── DistanceTestSlow.path │ │ │ ├── RotationTestFast.path │ │ │ ├── RotationTestSlow.path │ │ │ ├── OvalTestFast2.path │ │ │ ├── OvalTestSlow1.path │ │ │ ├── OvalTestSlow2.path │ │ │ └── OvalTestFast1.path │ ├── home.json │ ├── 2024-preseason-vision.json │ └── 2023-chargedup.json │ └── java │ └── frc │ ├── lib │ ├── team3015 │ │ └── subsystem │ │ │ ├── selfcheck │ │ │ ├── SelfChecking.java │ │ │ ├── SelfCheckingPWMMotor.java │ │ │ ├── SelfCheckingSparkMax.java │ │ │ └── SelfCheckingCANCoder.java │ │ │ └── SubsystemFault.java │ ├── team3061 │ │ ├── differential_drivetrain │ │ │ ├── DifferentialDrivetrainConstants.java │ │ │ └── DifferentialDrivetrainIO.java │ │ ├── swerve_drivetrain │ │ │ ├── SwerveDrivetrainConstants.java │ │ │ ├── SysIdSwerveSteerGains_Torque.java │ │ │ ├── SysIdSwerveTranslation_Torque.java │ │ │ └── swerve │ │ │ │ └── Conversions.java │ │ ├── gyro │ │ │ ├── GyroIO.java │ │ │ └── GyroIOPigeon2Phoenix6.java │ │ ├── vision │ │ │ ├── VisionIO.java │ │ │ ├── VisionIOSim.java │ │ │ ├── VisionConstants.java │ │ │ └── VisionIOPhotonVision.java │ │ ├── pneumatics │ │ │ ├── PneumaticsIO.java │ │ │ ├── PneumaticsIORev.java │ │ │ └── Pneumatics.java │ │ ├── leds │ │ │ ├── LEDsCANdle.java │ │ │ └── LEDsRIO.java │ │ ├── sim │ │ │ ├── VelocitySystemSim.java │ │ │ └── ElevatorSystemSim.java │ │ └── util │ │ │ ├── GeometryUtils.java │ │ │ ├── CustomPoseEstimator.java │ │ │ ├── SysIdRoutineChooser.java │ │ │ ├── DifferentialRobotOdometry.java │ │ │ └── SwerveRobotOdometry.java │ ├── team6328 │ │ └── util │ │ │ ├── LoggedTracer.java │ │ │ ├── NTClientLogger.java │ │ │ └── LoggedTunableNumber.java │ └── team254 │ │ ├── CurrentSpikeDetector.java │ │ ├── CANcoderConfigEquality.java │ │ ├── ChezyRepeatCommand.java │ │ └── ChezySequenceCommandGroup.java │ └── robot │ ├── FieldRegionConstants.java │ ├── subsystems │ ├── arm │ │ ├── ArmIOXRP.java │ │ ├── ArmIO.java │ │ └── ArmConstants.java │ ├── manipulator │ │ ├── ManipulatorIO.java │ │ └── ManipulatorConstants.java │ ├── elevator │ │ ├── ElevatorIO.java │ │ └── ElevatorConstants.java │ └── shooter │ │ ├── ShooterConstants.java │ │ └── ShooterIO.java │ ├── Main.java │ ├── commands │ ├── ElevatorCommandsFactory.java │ ├── ArmCommandFactory.java │ ├── DifferentialDrivetrainCommandFactory.java │ └── ArcadeDrive.java │ ├── operator_interface │ ├── OperatorDashboard.java │ ├── SingleHandheldOI.java │ ├── SimDualJoysticksOI.java │ ├── DualJoysticksOI.java │ ├── OperatorInterface.java │ ├── FullOperatorConsoleOI.java │ └── OISelector.java │ ├── configs │ └── VisionTestPlatformConfig.java │ ├── Constants.java │ ├── visualizations │ └── RobotVisualization.java │ └── Region2d.java ├── .github └── ISSUE_TEMPLATE │ ├── question.md │ ├── featureRequest.md │ └── bugReport.md ├── vh113.py ├── vendordeps ├── XRPVendordep.json ├── AdvantageKit.json ├── WPILibNewCommands.json ├── PathplannerLib.json ├── REVLib.json └── photonlib.json ├── settings.gradle ├── LICENSE ├── WPILib-License.md ├── simgui-ds.json ├── simgui.json ├── gradlew.bat └── .gitignore /networktables.json: -------------------------------------------------------------------------------- 1 | [] 2 | -------------------------------------------------------------------------------- /networktables.json.bck: -------------------------------------------------------------------------------- 1 | [ 2 | 3 | ] 4 | -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HuskieRobotics/3061-lib/HEAD/gradle/wrapper/gradle-wrapper.jar -------------------------------------------------------------------------------- /.wpilib/wpilib_preferences.json: -------------------------------------------------------------------------------- 1 | { 2 | "enableCppIntellisense": false, 3 | "currentLanguage": "java", 4 | "projectYear": "2025", 5 | "teamNumber": 3061 6 | } 7 | -------------------------------------------------------------------------------- /pull_latest_logs_from_rio.sh: -------------------------------------------------------------------------------- 1 | ssh lvuser@10.30.61.2 "ls -t /U/*.wpilog | head -n 4 | tar -cf /U/logdump.tar.gz -T -" 2 | scp lvuser@10.30.61.2:/U/logdump.tar.gz ~ 3 | tar -vxf ~/logdump.tar.gz 4 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | "recommendations": [ 3 | "richardwillis.vscode-spotless-gradle", 4 | "sonarsource.sonarlint-vscode", 5 | "streetsidesoftware.code-spell-checker", 6 | "onlyutkarsh.git-config-user-profiles" 7 | ] 8 | } 9 | -------------------------------------------------------------------------------- /src/main/deploy/example.txt: -------------------------------------------------------------------------------- 1 | Files placed in this directory will be deployed to the RoboRIO into the 2 | 'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function 3 | to get a proper path relative to the deploy directory. -------------------------------------------------------------------------------- /gradle/wrapper/gradle-wrapper.properties: -------------------------------------------------------------------------------- 1 | distributionBase=GRADLE_USER_HOME 2 | distributionPath=permwrapper/dists 3 | distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip 4 | networkTimeout=10000 5 | validateDistributionUrl=true 6 | zipStoreBase=GRADLE_USER_HOME 7 | zipStorePath=permwrapper/dists 8 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfChecking.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3015.subsystem.selfcheck; 2 | 3 | import frc.lib.team3015.subsystem.SubsystemFault; 4 | import java.util.List; 5 | 6 | public interface SelfChecking { 7 | List checkForFaults(); 8 | 9 | void clearStickyFaults(); 10 | } 11 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "0.2.0", 3 | "configurations": [ 4 | { 5 | "type": "wpilib", 6 | "name": "WPILib Desktop Debug", 7 | "request": "launch", 8 | "desktop": true 9 | }, 10 | { 11 | "type": "wpilib", 12 | "name": "WPILib roboRIO Debug", 13 | "request": "launch", 14 | "desktop": false 15 | } 16 | ] 17 | } 18 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/Start Point.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "Start Point" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": null, 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/Differential Auto.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "Differential Path" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "XRP", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/DistanceTestFast.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "DistanceTestFast" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "Tuning", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/DistanceTestMed.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "DistanceTestMed" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "Tuning", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/DistanceTestSlow.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "DistanceTestSlow" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "Tuning", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/RotationTestFast.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "RotationTestFast" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "Tuning", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/RotationTestSlow.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "RotationTestSlow" 11 | } 12 | } 13 | ] 14 | } 15 | }, 16 | "resetOdom": true, 17 | "folder": "Tuning", 18 | "choreoAuto": false 19 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/FieldRegionConstants.java: -------------------------------------------------------------------------------- 1 | package frc.robot; 2 | 3 | /** 4 | * This class defines the points for all of the regions that defines the field along with the 5 | * transitions points between these regions. For more information refer to the Field2d and Region2d 6 | * classes. 7 | */ 8 | public final class FieldRegionConstants { 9 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 10 | 11 | private FieldRegionConstants() { 12 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/OvalTestFast.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "OvalTestFast1" 11 | } 12 | }, 13 | { 14 | "type": "path", 15 | "data": { 16 | "pathName": "OvalTestFast2" 17 | } 18 | } 19 | ] 20 | } 21 | }, 22 | "resetOdom": true, 23 | "folder": "Tuning", 24 | "choreoAuto": false 25 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/autos/OvalTestSlow.auto: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "command": { 4 | "type": "sequential", 5 | "data": { 6 | "commands": [ 7 | { 8 | "type": "path", 9 | "data": { 10 | "pathName": "OvalTestSlow1" 11 | } 12 | }, 13 | { 14 | "type": "path", 15 | "data": { 16 | "pathName": "OvalTestSlow2" 17 | } 18 | } 19 | ] 20 | } 21 | }, 22 | "resetOdom": true, 23 | "folder": "Tuning", 24 | "choreoAuto": false 25 | } -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/question.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Question 3 | about: Ask about features of 3061-lib 4 | title: "" 5 | labels: question 6 | assignees: "" 7 | --- 8 | 9 | **Describe the question you have.** 10 | A clear and concise description of what you want clarification on. 11 | 12 | **Describe the reason for your confusion.** 13 | A clear and concise description of why the question requires clarification and what's confusing. 14 | 15 | **Is your question related to a problem? Please describe.** 16 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 17 | 18 | **Additional context** 19 | Add any other context or screenshots about the question here. 20 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/featureRequest.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for 3061-lib 4 | title: "" 5 | labels: enhancement 6 | assignees: "" 7 | --- 8 | 9 | **Is your feature request related to a problem? Please describe.** 10 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 11 | 12 | **Describe the solution you'd like** 13 | A clear and concise description of what you want to happen. 14 | 15 | **Describe alternatives you've considered** 16 | A clear and concise description of any alternative solutions or features you've considered. 17 | 18 | **Additional context** 19 | Add any other context or screenshots about the feature request here. 20 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/arm/ArmIOXRP.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.arm; 2 | 3 | import static edu.wpi.first.units.Units.*; 4 | 5 | import edu.wpi.first.units.measure.Angle; 6 | import edu.wpi.first.wpilibj.xrp.XRPServo; 7 | 8 | public class ArmIOXRP implements ArmIO { 9 | 10 | private final XRPServo armMotor = new XRPServo(4); 11 | 12 | private double referenceAngleDegrees = 0.0; 13 | 14 | @Override 15 | public void updateInputs(ArmIOInputs inputs) { 16 | inputs.angleDegrees = armMotor.getAngle(); 17 | inputs.angleMotorReferenceAngleDegrees = referenceAngleDegrees; 18 | } 19 | 20 | @Override 21 | public void setAngle(Angle angle) { 22 | armMotor.setAngle(angle.in(Degrees)); 23 | } 24 | } 25 | -------------------------------------------------------------------------------- /vh113.py: -------------------------------------------------------------------------------- 1 | import json 2 | import requests 3 | 4 | ip = '192.168.69.1' 5 | 6 | data = { 7 | 'blueVlans': '40_50_60', 8 | 'channel': 13, 9 | 'channelBandwidth': '40MHz', 10 | 'redVlans': '10_20_30', 11 | 'stationConfigurations': { 12 | 'blue1': { 13 | 'ssid': '3061-Calypso', 14 | 'wpaKey': '30613061' 15 | }, 16 | 'blue2': { 17 | 'ssid': '9999-Prac', 18 | 'wpaKey': '30613061' 19 | }, 20 | 'blue3': { 21 | 'ssid': '', 22 | 'wpaKey': '' 23 | }, 24 | 'red1': { 25 | 'ssid': '', 26 | 'wpaKey': '' 27 | }, 28 | 'red2': { 29 | 'ssid': '', 30 | 'wpaKey': '' 31 | }, 32 | 'red3': { 33 | 'ssid': '', 34 | 'wpaKey': '' 35 | } 36 | } 37 | } 38 | 39 | r = requests.post(f'http://{ip}/configuration', json.dumps(data)) 40 | 41 | print(r.status_code, r.reason) -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/differential_drivetrain/DifferentialDrivetrainConstants.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.differential_drivetrain; 2 | 3 | public class DifferentialDrivetrainConstants { 4 | 5 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 6 | 7 | private DifferentialDrivetrainConstants() { 8 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 9 | } 10 | 11 | public static final String SUBSYSTEM_NAME = "Drivetrain"; 12 | 13 | // XRP Differential Drivetrain Constants 14 | public static final double XRP_GEAR_RATIO = 15 | (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1 16 | public static final double XRP_COUNTS_PER_MOTOR_SHAFT_REV = 12.0; 17 | public static final double XRP_COUNTS_PER_REVOLUTION = 18 | XRP_COUNTS_PER_MOTOR_SHAFT_REV * XRP_GEAR_RATIO; // 585.0 19 | } 20 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/Main.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot; 6 | 7 | import edu.wpi.first.wpilibj.RobotBase; 8 | 9 | /** 10 | * Do NOT add any static variables to this class, or any initialization at all. Unless you know what 11 | * you are doing, do not modify this file except to change the parameter class to the startRobot 12 | * call. 13 | */ 14 | public final class Main { 15 | private Main() {} 16 | 17 | /** 18 | * Main initialization function. Do not perform any initialization here. 19 | * 20 | *

If you change your main robot class, change the parameter type. 21 | */ 22 | public static void main(String... args) { 23 | RobotBase.startRobot(Robot::new); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "robotWidth": 0.85725, 3 | "robotLength": 0.85725, 4 | "holonomicMode": true, 5 | "pathFolders": [ 6 | "XRP", 7 | "Tuning" 8 | ], 9 | "autoFolders": [ 10 | "XRP", 11 | "Tuning" 12 | ], 13 | "defaultMaxVel": 3.0, 14 | "defaultMaxAccel": 6.0, 15 | "defaultMaxAngVel": 540.0, 16 | "defaultMaxAngAccel": 720.0, 17 | "defaultNominalVoltage": 12.0, 18 | "robotMass": 66.45, 19 | "robotMOI": 6.985, 20 | "robotTrackwidth": 0.546, 21 | "driveWheelRadius": 0.05, 22 | "driveGearing": 5.903, 23 | "maxDriveSpeed": 4.67, 24 | "driveMotorType": "krakenX60", 25 | "driveCurrentLimit": 80.0, 26 | "wheelCOF": 1.5, 27 | "flModuleX": 0.289, 28 | "flModuleY": 0.289, 29 | "frModuleX": 0.289, 30 | "frModuleY": -0.289, 31 | "blModuleX": -0.289, 32 | "blModuleY": 0.289, 33 | "brModuleX": -0.289, 34 | "brModuleY": -0.289, 35 | "bumperOffsetX": 0.0, 36 | "bumperOffsetY": 0.0, 37 | "robotFeatures": [] 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team6328/util/LoggedTracer.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 FRC 6328 2 | // http://github.com/Mechanical-Advantage 3 | // 4 | // Use of this source code is governed by an MIT-style 5 | // license that can be found in the LICENSE file at 6 | // the root directory of this project. 7 | 8 | package frc.lib.team6328.util; 9 | 10 | import edu.wpi.first.wpilibj.Timer; 11 | import org.littletonrobotics.junction.Logger; 12 | 13 | /** Utility class for logging code execution times. */ 14 | public class LoggedTracer { 15 | private LoggedTracer() {} 16 | 17 | private static double startTime = -1.0; 18 | 19 | /** Reset the clock. */ 20 | public static void reset() { 21 | startTime = Timer.getFPGATimestamp(); 22 | } 23 | 24 | /** Save the time elapsed since the last reset or record. */ 25 | public static void record(String epochName) { 26 | double now = Timer.getFPGATimestamp(); 27 | Logger.recordOutput("LoggedTracer/" + epochName + "MS", (now - startTime) * 1000.0); 28 | startTime = now; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingPWMMotor.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3015.subsystem.selfcheck; 2 | 3 | import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController; 4 | import frc.lib.team3015.subsystem.SubsystemFault; 5 | import java.util.ArrayList; 6 | import java.util.List; 7 | 8 | public class SelfCheckingPWMMotor implements SelfChecking { 9 | private final String label; 10 | private final PWMMotorController motor; 11 | 12 | public SelfCheckingPWMMotor(String label, PWMMotorController motor) { 13 | this.label = label; 14 | this.motor = motor; 15 | } 16 | 17 | @Override 18 | public List checkForFaults() { 19 | List faults = new ArrayList<>(); 20 | 21 | if (!motor.isAlive()) { 22 | faults.add(new SubsystemFault(String.format("[%s]: Device timed out", label))); 23 | } 24 | 25 | return faults; 26 | } 27 | 28 | @Override 29 | public void clearStickyFaults() { 30 | // PWM motor doesn't have any sticky faults to clear 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /vendordeps/XRPVendordep.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "XRPVendordep.json", 3 | "name": "XRP-Vendordep", 4 | "version": "1.0.0", 5 | "uuid": "1571a1a5-ed3f-4f07-b7eb-b2beb17394e0", 6 | "frcYear": "2025", 7 | "mavenUrls": [], 8 | "jsonUrl": "", 9 | "javaDependencies": [ 10 | { 11 | "groupId": "edu.wpi.first.xrpVendordep", 12 | "artifactId": "xrpVendordep-java", 13 | "version": "wpilib" 14 | } 15 | ], 16 | "jniDependencies": [], 17 | "cppDependencies": [ 18 | { 19 | "groupId": "edu.wpi.first.xrpVendordep", 20 | "artifactId": "xrpVendordep-cpp", 21 | "version": "wpilib", 22 | "libName": "xrpVendordep", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": true, 26 | "skipInvalidPlatforms": true, 27 | "binaryPlatforms": [ 28 | "linuxarm32", 29 | "linuxarm64", 30 | "windowsx86-64", 31 | "windowsx86", 32 | "linuxx86-64", 33 | "osxuniversal" 34 | ] 35 | } 36 | ] 37 | } 38 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingSparkMax.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3015.subsystem.selfcheck; 2 | 3 | import com.revrobotics.REVLibError; 4 | import com.revrobotics.spark.SparkMax; 5 | import frc.lib.team3015.subsystem.SubsystemFault; 6 | import java.util.ArrayList; 7 | import java.util.List; 8 | 9 | public class SelfCheckingSparkMax implements SelfChecking { 10 | private final String label; 11 | private SparkMax spark; 12 | 13 | public SelfCheckingSparkMax(String label, SparkMax spark) { 14 | this.label = label; 15 | this.spark = spark; 16 | } 17 | 18 | @Override 19 | public List checkForFaults() { 20 | ArrayList faults = new ArrayList<>(); 21 | 22 | REVLibError err = spark.getLastError(); 23 | if (err != REVLibError.kOk) { 24 | faults.add(new SubsystemFault(String.format("[%s]: Error: %s", label, err.name()))); 25 | } 26 | 27 | return faults; 28 | } 29 | 30 | @Override 31 | public void clearStickyFaults() { 32 | spark.clearFaults(); 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /vendordeps/AdvantageKit.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "AdvantageKit.json", 3 | "name": "AdvantageKit", 4 | "version": "4.1.2", 5 | "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" 9 | ], 10 | "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "org.littletonrobotics.akit", 14 | "artifactId": "akit-java", 15 | "version": "4.1.2" 16 | } 17 | ], 18 | "jniDependencies": [ 19 | { 20 | "groupId": "org.littletonrobotics.akit", 21 | "artifactId": "akit-wpilibio", 22 | "version": "4.1.2", 23 | "skipInvalidPlatforms": false, 24 | "isJar": false, 25 | "validPlatforms": [ 26 | "linuxathena", 27 | "linuxx86-64", 28 | "linuxarm64", 29 | "osxuniversal", 30 | "windowsx86-64" 31 | ] 32 | } 33 | ], 34 | "cppDependencies": [] 35 | } 36 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bugReport.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: "" 5 | labels: bug 6 | assignees: "" 7 | --- 8 | 9 | **Describe the bug** 10 | A clear and concise description of what the bug is. 11 | 12 | **To Reproduce** 13 | Steps to reproduce the behavior: 14 | 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Versions: (please complete the following information):** 27 | 28 | - OS: [e.g. Windows 11] 29 | - Project Information: [In Visual Studio Code, press the WPILib button and choose WPILib: Open Project Information. Press the copy button and paste the data here. If not using VS Code, please include WPILib version, Gradle version, Java version, C++ version (if applicable), and any third party libraries and versions] 30 | 31 | **Additional context** 32 | Add any other context about the problem here. 33 | -------------------------------------------------------------------------------- /vendordeps/WPILibNewCommands.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "WPILibNewCommands.json", 3 | "name": "WPILib-New-Commands", 4 | "version": "1.0.0", 5 | "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", 6 | "frcYear": "2025", 7 | "mavenUrls": [], 8 | "jsonUrl": "", 9 | "javaDependencies": [ 10 | { 11 | "groupId": "edu.wpi.first.wpilibNewCommands", 12 | "artifactId": "wpilibNewCommands-java", 13 | "version": "wpilib" 14 | } 15 | ], 16 | "jniDependencies": [], 17 | "cppDependencies": [ 18 | { 19 | "groupId": "edu.wpi.first.wpilibNewCommands", 20 | "artifactId": "wpilibNewCommands-cpp", 21 | "version": "wpilib", 22 | "libName": "wpilibNewCommands", 23 | "headerClassifier": "headers", 24 | "sourcesClassifier": "sources", 25 | "sharedLibrary": true, 26 | "skipInvalidPlatforms": true, 27 | "binaryPlatforms": [ 28 | "linuxathena", 29 | "linuxarm32", 30 | "linuxarm64", 31 | "windowsx86-64", 32 | "windowsx86", 33 | "linuxx86-64", 34 | "osxuniversal" 35 | ] 36 | } 37 | ] 38 | } 39 | -------------------------------------------------------------------------------- /settings.gradle: -------------------------------------------------------------------------------- 1 | import org.gradle.internal.os.OperatingSystem 2 | 3 | pluginManagement { 4 | repositories { 5 | mavenLocal() 6 | gradlePluginPortal() 7 | String frcYear = '2025' 8 | File frcHome 9 | if (OperatingSystem.current().isWindows()) { 10 | String publicFolder = System.getenv('PUBLIC') 11 | if (publicFolder == null) { 12 | publicFolder = "C:\\Users\\Public" 13 | } 14 | def homeRoot = new File(publicFolder, "wpilib") 15 | frcHome = new File(homeRoot, frcYear) 16 | } else { 17 | def userFolder = System.getProperty("user.home") 18 | def homeRoot = new File(userFolder, "wpilib") 19 | frcHome = new File(homeRoot, frcYear) 20 | } 21 | def frcHomeMaven = new File(frcHome, 'maven') 22 | maven { 23 | name 'frcHome' 24 | url frcHomeMaven 25 | } 26 | } 27 | } 28 | 29 | Properties props = System.getProperties(); 30 | props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); 31 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/swerve_drivetrain/SwerveDrivetrainConstants.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.swerve_drivetrain; 2 | 3 | public class SwerveDrivetrainConstants { 4 | 5 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 6 | 7 | private SwerveDrivetrainConstants() { 8 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 9 | } 10 | 11 | public static final String SUBSYSTEM_NAME = "Drivetrain"; 12 | 13 | public static final boolean ENABLE_TELEPORT_DETECTION = false; 14 | public static final double TELEPORT_DETECTION_THRESHOLD_METERS = 0.4; 15 | 16 | public static final double DEMO_MODE_MAX_VELOCITY = 0.5; 17 | 18 | public static final double TILT_THRESHOLD_DEG = 5.0; 19 | public static final double UNTILT_VELOCITY_MPS = 0.5; 20 | 21 | public static final double SYSTEM_TEST_VELOCITY_TOLERANCE = 0.25; 22 | public static final double SYSTEM_TEST_ANGLE_TOLERANCE_DEG = 1.25; 23 | 24 | public enum SysIDCharacterizationMode { 25 | TRANSLATION_VOLTS, 26 | TRANSLATION_CURRENT, 27 | STEER_VOLTS, 28 | STEER_CURRENT, 29 | ROTATION_VOLTS 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022-2025 Huskie Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /vendordeps/PathplannerLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "PathplannerLib.json", 3 | "name": "PathplannerLib", 4 | "version": "2025.2.5", 5 | "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://3015rangerrobotics.github.io/pathplannerlib/repo" 9 | ], 10 | "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "com.pathplanner.lib", 14 | "artifactId": "PathplannerLib-java", 15 | "version": "2025.2.5" 16 | } 17 | ], 18 | "jniDependencies": [], 19 | "cppDependencies": [ 20 | { 21 | "groupId": "com.pathplanner.lib", 22 | "artifactId": "PathplannerLib-cpp", 23 | "version": "2025.2.5", 24 | "libName": "PathplannerLib", 25 | "headerClassifier": "headers", 26 | "sharedLibrary": false, 27 | "skipInvalidPlatforms": true, 28 | "binaryPlatforms": [ 29 | "windowsx86-64", 30 | "linuxx86-64", 31 | "osxuniversal", 32 | "linuxathena", 33 | "linuxarm32", 34 | "linuxarm64" 35 | ] 36 | } 37 | ] 38 | } 39 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3015/subsystem/SubsystemFault.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3015.subsystem; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import java.util.Objects; 5 | 6 | public class SubsystemFault { 7 | public final String description; 8 | public final double timestamp; 9 | public final boolean isWarning; 10 | 11 | public SubsystemFault(String description) { 12 | this(description, false); 13 | } 14 | 15 | public SubsystemFault(String description, boolean isWarning) { 16 | this.description = description; 17 | this.timestamp = Timer.getTimestamp(); 18 | this.isWarning = isWarning; 19 | } 20 | 21 | @Override 22 | public boolean equals(Object other) { 23 | if (this == other) { 24 | return true; 25 | } 26 | 27 | if (other == null) { 28 | return false; 29 | } 30 | 31 | if (getClass() != other.getClass()) { 32 | return false; 33 | } 34 | 35 | SubsystemFault otherSubsystemFault = (SubsystemFault) other; 36 | 37 | return description.equals(otherSubsystemFault.description) 38 | && isWarning == otherSubsystemFault.isWarning; 39 | } 40 | 41 | @Override 42 | public int hashCode() { 43 | return Objects.hash(description, timestamp, isWarning); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/commands/ElevatorCommandsFactory.java: -------------------------------------------------------------------------------- 1 | package frc.robot.commands; 2 | 3 | import edu.wpi.first.wpilibj2.command.Commands; 4 | import frc.robot.operator_interface.OperatorInterface; 5 | import frc.robot.subsystems.elevator.Elevator; 6 | 7 | public class ElevatorCommandsFactory { 8 | 9 | private ElevatorCommandsFactory() {} 10 | 11 | public static void registerCommands(OperatorInterface oi, Elevator elevator) { 12 | 13 | oi.getRaiseElevatorSlowButton() 14 | .onTrue( 15 | Commands.runOnce(elevator::raiseElevatorSlow, elevator) 16 | .withName("raise elevator slow")); 17 | 18 | oi.getRaiseElevatorSlowButton() 19 | .onFalse(Commands.runOnce(elevator::stop, elevator).withName("stop elevator")); 20 | 21 | oi.getLowerElevatorSlowButton() 22 | .onTrue( 23 | Commands.runOnce(elevator::lowerElevatorSlow, elevator) 24 | .withName("lower elevator slow")); 25 | oi.getLowerElevatorSlowButton() 26 | .onFalse( 27 | Commands.sequence( 28 | Commands.runOnce(elevator::stop, elevator), 29 | Commands.runOnce(elevator::zero, elevator)) 30 | .withName("stop and zero elevator")); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/Differential Path.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 2.0, 7 | "y": 7.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 3.0, 12 | "y": 7.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 4.0, 20 | "y": 6.0 21 | }, 22 | "prevControl": { 23 | "x": 3.0, 24 | "y": 6.0 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 0.5, 37 | "maxAcceleration": 0.5, 38 | "maxAngularVelocity": 540.0, 39 | "maxAngularAcceleration": 720.0, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 0.0 46 | }, 47 | "reversed": false, 48 | "folder": "XRP", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/Start Point.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 1.25, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 2.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 1.75, 24 | "y": 1.0 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 1.0, 37 | "maxAcceleration": 11.805, 38 | "maxAngularVelocity": 537.0, 39 | "maxAngularAcceleration": 2303.0, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 0.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/java/frc/robot/commands/ArmCommandFactory.java: -------------------------------------------------------------------------------- 1 | package frc.robot.commands; 2 | 3 | import static edu.wpi.first.units.Units.*; 4 | 5 | import edu.wpi.first.wpilibj2.command.InstantCommand; 6 | import frc.robot.operator_interface.OperatorInterface; 7 | import frc.robot.subsystems.arm.Arm; 8 | 9 | public class ArmCommandFactory { 10 | 11 | private ArmCommandFactory() {} 12 | 13 | public static void registerCommands(OperatorInterface oi, Arm arm) { 14 | 15 | oi.getMoveArmMiddlePositionTrigger() 16 | .onTrue( 17 | new InstantCommand(() -> arm.setAngle(Degrees.of(45.0)), arm) 18 | .withName("Move Arm to Middle Position")); 19 | oi.getMoveArmMiddlePositionTrigger() 20 | .onFalse( 21 | new InstantCommand(() -> arm.setAngle(Degrees.of(0.0)), arm) 22 | .withName("Reset Arm to Zero Position")); 23 | 24 | oi.getMoveArmHighPositionTrigger() 25 | .onTrue( 26 | new InstantCommand(() -> arm.setAngle(Degrees.of(90.0)), arm) 27 | .withName("Move Arm to High Position")); 28 | oi.getMoveArmHighPositionTrigger() 29 | .onFalse( 30 | new InstantCommand(() -> arm.setAngle(Degrees.of(0.0)), arm) 31 | .withName("Reset Arm to Zero Position")); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/DistanceTestFast.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 2.118033988749895, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 5.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.4793093674254454, 24 | "y": 1.0000000000000002 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 3.0, 37 | "maxAcceleration": 6.0, 38 | "maxAngularVelocity": 540.0, 39 | "maxAngularAcceleration": 720.0, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 0.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": true 54 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/DistanceTestMed.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 2.118033988749895, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 5.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.4793093674254454, 24 | "y": 1.0000000000000002 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 2.0, 37 | "maxAcceleration": 3.5, 38 | "maxAngularVelocity": 530.0, 39 | "maxAngularAcceleration": 510.0, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 0.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/DistanceTestSlow.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 2.118033988749895, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 5.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.4793093674254454, 24 | "y": 1.0000000000000002 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 1.0, 37 | "maxAcceleration": 1.5, 38 | "maxAngularVelocity": 561.96, 39 | "maxAngularAcceleration": 2066.2, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 0.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/RotationTestFast.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 2.118033988749895, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 5.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.4793093674254454, 24 | "y": 1.0000000000000002 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 1.0, 37 | "maxAcceleration": 1.5, 38 | "maxAngularVelocity": 561.96, 39 | "maxAngularAcceleration": 2066.2, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 180.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/RotationTestSlow.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 1.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 2.118033988749895, 12 | "y": 1.0 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 5.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.4793093674254454, 24 | "y": 1.0000000000000002 25 | }, 26 | "nextControl": null, 27 | "isLocked": false, 28 | "linkedName": null 29 | } 30 | ], 31 | "rotationTargets": [], 32 | "constraintZones": [], 33 | "pointTowardsZones": [], 34 | "eventMarkers": [], 35 | "globalConstraints": { 36 | "maxVelocity": 2.0, 37 | "maxAcceleration": 3.0, 38 | "maxAngularVelocity": 530.0, 39 | "maxAngularAcceleration": 510.0, 40 | "nominalVoltage": 12.0, 41 | "unlimited": false 42 | }, 43 | "goalEndState": { 44 | "velocity": 0, 45 | "rotation": 180.0 46 | }, 47 | "reversed": false, 48 | "folder": "Tuning", 49 | "idealStartingState": { 50 | "velocity": 0, 51 | "rotation": 0.0 52 | }, 53 | "useDefaultConstraints": false 54 | } -------------------------------------------------------------------------------- /src/main/deploy/home.json: -------------------------------------------------------------------------------- 1 | { 2 | "tags": [ 3 | { 4 | "ID": 12, 5 | "pose": { 6 | "translation": { 7 | "x": 1.7018, 8 | "y": 4.2926, 9 | "z": 1.14935 10 | }, 11 | "rotation": { 12 | "quaternion": { 13 | "W": -0.7071067811865475, 14 | "X": 0.0, 15 | "Y": 0.0, 16 | "Z": 0.7071067811865476 17 | } 18 | } 19 | } 20 | }, 21 | { 22 | "ID": 7, 23 | "pose": { 24 | "translation": { 25 | "x": 0.0, 26 | "y": 3.61515, 27 | "z": 0.064 28 | }, 29 | "rotation": { 30 | "quaternion": { 31 | "W": 1.0, 32 | "X": 0.0, 33 | "Y": 0.0, 34 | "Z": 0.0 35 | } 36 | } 37 | } 38 | }, 39 | { 40 | "ID": 8, 41 | "pose": { 42 | "translation": { 43 | "x": 0.0, 44 | "y": 3.05, 45 | "z": 0.064 46 | }, 47 | "rotation": { 48 | "quaternion": { 49 | "W": 1.0, 50 | "X": 0.0, 51 | "Y": 0.0, 52 | "Z": 0.0 53 | } 54 | } 55 | } 56 | } 57 | ], 58 | "field": { 59 | "length": 10.7188, 60 | "width": 3.175 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/manipulator/ManipulatorIO.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.manipulator; 2 | 3 | import edu.wpi.first.units.measure.Voltage; 4 | import org.littletonrobotics.junction.AutoLog; 5 | 6 | /** Generic subsystem hardware interface. */ 7 | public interface ManipulatorIO { 8 | // The inputs class for a subsystem usually contains the stator and supply currents, temperature, 9 | // voltage (or current), and, depending on the control mode, additional fields related to position 10 | // or velocity (both measured and reference). The first property is always `connected` and logs if 11 | // each device is reachable. Due to logging limitations, properties cannot be a subtype of 12 | // Measure. Therefore all properties are suffix with their unit to mitigate bugs due to unit 13 | // mismatches. 14 | @AutoLog 15 | public static class ManipulatorIOInputs { 16 | 17 | boolean manipulatorConnected = false; 18 | boolean isManipulatorPrimaryIRBlocked = false; 19 | boolean isManipulatorSecondaryIRBlocked = false; 20 | double manipulatorStatorCurrentAmps = 0; 21 | double manipulatorSupplyCurrentAmps = 0; 22 | double manipulatorTempCelsius = 0; 23 | double manipulatorMotorVoltage = 0; 24 | double manipulatorVelocityRPS = 0; 25 | } 26 | 27 | public default void updateInputs(ManipulatorIOInputs inputs) {} 28 | 29 | public default void setManipulatorVoltage(Voltage volts) {} 30 | } 31 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/gyro/GyroIO.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Initially from https://github.com/Mechanical-Advantage/SwerveDevelopment 3 | */ 4 | 5 | package frc.lib.team3061.gyro; 6 | 7 | import org.littletonrobotics.junction.AutoLog; 8 | 9 | @java.lang.SuppressWarnings({"java:S1104"}) 10 | 11 | /** 12 | * Gyro hardware abstraction interface 13 | * 14 | *

The coordinate system for the gyro interface is the same as the Pigeon 2.0's coordinate 15 | * system: https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf 16 | * 17 | *

The Gyro is modeled not as a subsystem but rather a shared resource that may be used by 18 | * multiple subsystems. Since it doesn't have a periodic method like a subsystem, it is important 19 | * that its updateInputs method is called just once via another periodic method. (This requires some 20 | * coordination.) 21 | */ 22 | public interface GyroIO { 23 | @AutoLog 24 | public static class GyroIOInputs { 25 | public boolean connected = false; 26 | public double yawDeg = 0.0; 27 | public double yawDegPerSec = 0.0; 28 | public double pitchDeg = 0.0; 29 | public double pitchDegPerSec = 0.0; 30 | public double rollDeg = 0.0; 31 | public double rollDegPerSec = 0.0; 32 | } 33 | 34 | /** 35 | * Updates the set of loggable inputs. 36 | * 37 | * @param inputs the inputs to update 38 | */ 39 | public default void updateInputs(GyroIOInputs inputs) {} 40 | } 41 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/commands/DifferentialDrivetrainCommandFactory.java: -------------------------------------------------------------------------------- 1 | package frc.robot.commands; 2 | 3 | import frc.lib.team3061.differential_drivetrain.DifferentialDrivetrain; 4 | import frc.robot.operator_interface.OperatorInterface; 5 | 6 | public class DifferentialDrivetrainCommandFactory { 7 | 8 | private DifferentialDrivetrainCommandFactory() {} 9 | 10 | public static void registerCommands( 11 | OperatorInterface oi, DifferentialDrivetrain differentialDrivetrain) { 12 | /* 13 | * Set up the default command for the drivetrain. The joysticks' values map to percentage of the 14 | * maximum velocities. The velocities may be specified from either the robot's frame of 15 | * reference or the field's frame of reference. In the robot's frame of reference, the positive 16 | * x direction is forward; the positive y direction, left; position rotation, CCW. In the field 17 | * frame of reference, the origin of the field to the lower left corner (i.e., the corner of the 18 | * field to the driver's right). Zero degrees is away from the driver and increases in the CCW 19 | * direction. This is why the left joystick's y axis specifies the velocity in the x direction 20 | * and the left joystick's x axis specifies the velocity in the y direction. 21 | */ 22 | differentialDrivetrain.setDefaultCommand( 23 | new ArcadeDrive(differentialDrivetrain, oi::getTranslateX, oi::getRotate)); 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/arm/ArmIO.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.arm; 2 | 3 | import edu.wpi.first.units.measure.Angle; 4 | import org.littletonrobotics.junction.AutoLog; 5 | 6 | public interface ArmIO { 7 | 8 | // The inputs class for a subsystem usually contains the stator and supply currents, temperature, 9 | // voltage (or current), and, depending on the control mode, additional fields related to position 10 | // or velocity (both measured and reference). The first property is always `connected` and logs if 11 | // each device is reachable. Due to logging limitations, properties cannot be a subtype of 12 | // Measure. Therefore all properties are suffix with their unit to mitigate bugs due to unit 13 | // mismatches. 14 | @AutoLog 15 | public static class ArmIOInputs { 16 | boolean connected = false; 17 | double angleMotorStatorCurrentAmps = 0.0; 18 | double angleMotorSupplyCurrentAmps = 0.0; 19 | double angleMotorTemperatureCelsius = 0.0; 20 | double angleMotorVoltage = 0.0; 21 | double angleDegrees = 0.0; 22 | double angleMotorReferenceAngleDegrees = 0.0; 23 | double angleMotorClosedLoopReferenceAngleDegrees = 0.0; 24 | double angleMotorClosedLoopErrorAngleDegrees = 0.0; 25 | } 26 | 27 | /** Updates the set of loggable inputs. */ 28 | public default void updateInputs(ArmIOInputs inputs) {} 29 | 30 | public default void setAngleMotorVoltage(double voltage) {} 31 | 32 | public default void setAngle(Angle angle) {} 33 | } 34 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team254/CurrentSpikeDetector.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team254; 2 | 3 | import edu.wpi.first.wpilibj.Timer; 4 | import edu.wpi.first.wpilibj2.command.button.Trigger; 5 | import java.util.function.BooleanSupplier; 6 | 7 | public class CurrentSpikeDetector implements BooleanSupplier { 8 | private double currentThresholdAmps; 9 | private double timeThresholdSeconds; 10 | private Timer currentOverThresholdTimer; 11 | private Trigger cachedTrigger; 12 | private boolean lastValue = false; 13 | 14 | public CurrentSpikeDetector(double currentThresholdAmps, double timeThresholdSeconds) { 15 | this.currentThresholdAmps = currentThresholdAmps; 16 | this.timeThresholdSeconds = timeThresholdSeconds; 17 | this.currentOverThresholdTimer = new Timer(); 18 | } 19 | 20 | public boolean update(double current) { 21 | if (current > currentThresholdAmps) { 22 | currentOverThresholdTimer.start(); 23 | 24 | boolean newValue = currentOverThresholdTimer.hasElapsed(timeThresholdSeconds); 25 | lastValue = newValue; 26 | return newValue; 27 | } else { 28 | currentOverThresholdTimer.stop(); 29 | currentOverThresholdTimer.reset(); 30 | lastValue = false; 31 | return false; 32 | } 33 | } 34 | 35 | public boolean getAsBoolean() { 36 | return lastValue; 37 | } 38 | 39 | public Trigger asTrigger() { 40 | if (cachedTrigger == null) { 41 | cachedTrigger = new Trigger(this); 42 | } 43 | return cachedTrigger; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/OperatorDashboard.java: -------------------------------------------------------------------------------- 1 | package frc.robot.operator_interface; 2 | 3 | import edu.wpi.first.wpilibj2.command.button.Trigger; 4 | import frc.lib.team6328.util.LoggedTunableBoolean; 5 | 6 | /** 7 | * OperatorDashboard is a class that implements the OperatorInterface. It is not a joystick, 8 | * controller, or physical button panel. Instead, it is a software-based dashboard with virtual 9 | * buttons. It is designed to be used by the operator on a handheld touchscreen running Elastic. For 10 | * a more sophisticated example, including implementing groups of buttons where only one may be 11 | * selected at a time, refer to the 2025 Huskie Robotics code base. 12 | */ 13 | public class OperatorDashboard implements OperatorInterface { 14 | // the readAndWrite argument must be true or else the dashboard will not work when tuning mode is 15 | // disabled. 16 | public final LoggedTunableBoolean enableVision = 17 | new LoggedTunableBoolean("operatorDashboard/Enable Vision", true, true); 18 | public final LoggedTunableBoolean enablePrimaryIRSensors = 19 | new LoggedTunableBoolean("operatorDashboard/Enable Primary IR Sensors", true, true); 20 | 21 | public OperatorDashboard() {} 22 | 23 | @Override 24 | public Trigger getVisionIsEnabledTrigger() { 25 | return new Trigger(() -> enableVision.get()); 26 | } 27 | 28 | @Override 29 | public Trigger getEnablePrimaryIRSensorsTrigger() { 30 | return new Trigger(() -> enablePrimaryIRSensors.get()); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/vision/VisionIO.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.vision; 2 | 3 | import edu.wpi.first.math.geometry.Pose3d; 4 | import org.littletonrobotics.junction.AutoLog; 5 | 6 | /** 7 | * The hardware abstraction interface for a PhotonVision-based co-processor that provides 8 | * PhotonPipelineResult objects. There is a one-to-one relationship between each VisionIO object and 9 | * each co-processor (e.g., Raspberry Pi) running PhotonVision. 10 | * 11 | *

In the future, this interface may be further abstracted to not be coupled to PhotonVision. 12 | * Currently, the abstraction is used to simulate vision. 13 | */ 14 | public interface VisionIO { 15 | @AutoLog 16 | public static class VisionIOInputs { 17 | boolean connected; 18 | PoseObservation[] poseObservations = new PoseObservation[0]; 19 | } 20 | 21 | /** Represents a robot pose sample used for pose estimation. */ 22 | public static record PoseObservation( 23 | double timestamp, 24 | Pose3d cameraPose, 25 | double latencySecs, 26 | double averageAmbiguity, 27 | double reprojectionError, 28 | long tagsSeenBitMap, 29 | int numTags, 30 | double averageTagDistance, 31 | PoseObservationType type) {} 32 | 33 | public enum PoseObservationType { 34 | SINGLE_TAG, 35 | MULTI_TAG 36 | } 37 | 38 | /** 39 | * Updates the set of loggable inputs. 40 | * 41 | * @param inputs the inputs to update 42 | */ 43 | public default void updateInputs(VisionIOInputs inputs) {} 44 | } 45 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.manipulator; 2 | 3 | public class ManipulatorConstants { 4 | 5 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 6 | 7 | private ManipulatorConstants() { 8 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 9 | } 10 | 11 | public static final String SUBSYSTEM_NAME = "Manipulator"; 12 | 13 | public static final int MANIPULATOR_MOTOR_ID = 56; 14 | public static final int MANIPULATOR_IR_SENSOR_ID = 0; 15 | public static final int MANIPULATOR_IR_BACKUP_SENSOR_ID = 1; 16 | 17 | // the following are determined based on the mechanical design of the arm 18 | public static final boolean MANIPULATOR_MOTOR_INVERTED = false; 19 | public static final double MANIPULATOR_GEAR_RATIO = 1.0; 20 | 21 | // voltages are determined empirically through tuning 22 | public static final double MANIPULATOR_COLLECTION_VOLTAGE = 4.0; 23 | public static final double MANIPULATOR_RELEASE_VOLTAGE = 4.0; 24 | public static final double MANIPULATOR_EJECT_VOLTAGE = -12.0; 25 | 26 | // used to trigger state transitions 27 | public static final double COLLECTION_TIME_OUT = 2.0; 28 | public static final double EJECT_DURATION_SECONDS = 0.5; 29 | 30 | // current limits are determined based on current budget for the robot 31 | public static final double MANIPULATOR_MOTOR_PEAK_CURRENT_LIMIT = 40; 32 | public static final double COLLECTION_CURRENT_SPIKE_THRESHOLD = 35.0; 33 | public static final double COLLECTION_CURRENT_TIME_THRESHOLD_SECONDS = 0.1; 34 | } 35 | -------------------------------------------------------------------------------- /WPILib-License.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009-2025 FIRST and other WPILib contributors 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | - Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | - Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | - Neither the name of FIRST, WPILib, nor the names of other WPILib 13 | contributors may be used to endorse or promote products derived from 14 | this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR 19 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/OvalTestFast2.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 5.0, 7 | "y": 2.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 4.180847955711008, 12 | "y": 1.4264235636489535 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 3.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.75, 24 | "y": 1.0 25 | }, 26 | "nextControl": { 27 | "x": 2.25, 28 | "y": 1.0 29 | }, 30 | "isLocked": false, 31 | "linkedName": null 32 | }, 33 | { 34 | "anchor": { 35 | "x": 1.0, 36 | "y": 2.0 37 | }, 38 | "prevControl": { 39 | "x": 1.895510319613444, 40 | "y": 1.3729569236620796 41 | }, 42 | "nextControl": null, 43 | "isLocked": false, 44 | "linkedName": null 45 | } 46 | ], 47 | "rotationTargets": [], 48 | "constraintZones": [], 49 | "pointTowardsZones": [], 50 | "eventMarkers": [], 51 | "globalConstraints": { 52 | "maxVelocity": 2.5, 53 | "maxAcceleration": 3.0, 54 | "maxAngularVelocity": 561.96, 55 | "maxAngularAcceleration": 2066.2, 56 | "nominalVoltage": 12.0, 57 | "unlimited": false 58 | }, 59 | "goalEndState": { 60 | "velocity": 0, 61 | "rotation": 0.0 62 | }, 63 | "reversed": false, 64 | "folder": "Tuning", 65 | "idealStartingState": { 66 | "velocity": 0, 67 | "rotation": 0.0 68 | }, 69 | "useDefaultConstraints": false 70 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/OvalTestSlow1.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 2.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 1.819152044288992, 12 | "y": 2.573576436351046 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 3.0, 20 | "y": 3.0 21 | }, 22 | "prevControl": { 23 | "x": 2.25, 24 | "y": 3.0 25 | }, 26 | "nextControl": { 27 | "x": 3.75, 28 | "y": 3.0 29 | }, 30 | "isLocked": false, 31 | "linkedName": null 32 | }, 33 | { 34 | "anchor": { 35 | "x": 5.0, 36 | "y": 2.0 37 | }, 38 | "prevControl": { 39 | "x": 4.196257919204226, 40 | "y": 2.5949778714860567 41 | }, 42 | "nextControl": null, 43 | "isLocked": false, 44 | "linkedName": null 45 | } 46 | ], 47 | "rotationTargets": [], 48 | "constraintZones": [], 49 | "pointTowardsZones": [], 50 | "eventMarkers": [], 51 | "globalConstraints": { 52 | "maxVelocity": 1.0, 53 | "maxAcceleration": 1.5, 54 | "maxAngularVelocity": 561.96, 55 | "maxAngularAcceleration": 2066.2, 56 | "nominalVoltage": 12.0, 57 | "unlimited": false 58 | }, 59 | "goalEndState": { 60 | "velocity": 0, 61 | "rotation": 180.0 62 | }, 63 | "reversed": false, 64 | "folder": "Tuning", 65 | "idealStartingState": { 66 | "velocity": 0, 67 | "rotation": 0.0 68 | }, 69 | "useDefaultConstraints": false 70 | } -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/OvalTestSlow2.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 5.0, 7 | "y": 2.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 4.180847955711008, 12 | "y": 1.4264235636489535 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 3.0, 20 | "y": 1.0 21 | }, 22 | "prevControl": { 23 | "x": 3.75, 24 | "y": 1.0 25 | }, 26 | "nextControl": { 27 | "x": 2.25, 28 | "y": 1.0 29 | }, 30 | "isLocked": false, 31 | "linkedName": null 32 | }, 33 | { 34 | "anchor": { 35 | "x": 1.0, 36 | "y": 2.0 37 | }, 38 | "prevControl": { 39 | "x": 1.895510319613444, 40 | "y": 1.3729569236620796 41 | }, 42 | "nextControl": null, 43 | "isLocked": false, 44 | "linkedName": null 45 | } 46 | ], 47 | "rotationTargets": [], 48 | "constraintZones": [], 49 | "pointTowardsZones": [], 50 | "eventMarkers": [], 51 | "globalConstraints": { 52 | "maxVelocity": 1.0, 53 | "maxAcceleration": 1.5, 54 | "maxAngularVelocity": 561.96, 55 | "maxAngularAcceleration": 2066.2, 56 | "nominalVoltage": 12.0, 57 | "unlimited": false 58 | }, 59 | "goalEndState": { 60 | "velocity": 0, 61 | "rotation": 0.0 62 | }, 63 | "reversed": false, 64 | "folder": "Tuning", 65 | "idealStartingState": { 66 | "velocity": 0, 67 | "rotation": 0.0 68 | }, 69 | "useDefaultConstraints": false 70 | } -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/pneumatics/PneumaticsIO.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.lib.team3061.pneumatics; 10 | 11 | import org.littletonrobotics.junction.AutoLog; 12 | 13 | @java.lang.SuppressWarnings({"java:S1104"}) 14 | 15 | /** 16 | * Pneumatics subsystem hardware abstraction interface. 17 | * 18 | *

The pneumatics hardware abstraction is designed to for two pressure sensors. One measures the 19 | * pressure upstream of the regulator (high pressure); the other, pressure downstream of the 20 | * regulator (low pressure). In addition, there is a flow sensor. (We use the SMC PFM711-N7-C-R.) 21 | */ 22 | public interface PneumaticsIO { 23 | @AutoLog 24 | public static class PneumaticsIOInputs { 25 | double highPressurePSI = 0.0; 26 | double lowPressurePSI = 0.0; 27 | boolean compressorActive = false; 28 | double compressorCurrentAmps = 0.0; 29 | double flowLPM = 0.0; 30 | double volumeL = 0.0; 31 | } 32 | 33 | /** Updates the set of loggable inputs. */ 34 | public default void updateInputs(PneumaticsIOInputs inputs) {} 35 | 36 | /** 37 | * Updates the compressor threshold. 38 | * 39 | *

This method enables different pressures to be used for different periods of the robot's 40 | * operation. For example, the end-game may have different requirements than the rest of the 41 | * match. 42 | * 43 | * @param useLow if true, use the low pressure thresholds defined in PneumaticsConstants; 44 | * otherwise, use the high pressure thresholds 45 | */ 46 | public default void useLowClosedLoopThresholds(boolean useLow) {} 47 | } 48 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team254/CANcoderConfigEquality.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 FRC 254 2 | // https://github.com/Team254/FRC-2023-Public 3 | // 4 | // Use of this source code is governed by an MIT-style 5 | // license that can be found in the LICENSE file at 6 | // the root directory of this project. 7 | // 8 | // Modeled after 254's TalonFXConfigEquality class 9 | 10 | package frc.lib.team254; 11 | 12 | import com.ctre.phoenix6.configs.*; 13 | import edu.wpi.first.math.MathUtil; 14 | 15 | @java.lang.SuppressWarnings({"java:S106"}) 16 | public class CANcoderConfigEquality { 17 | 18 | public static final double PHOENIX_CONFIG_EPSILON = 0.05; 19 | 20 | /** 21 | * Returns true if the two specified CANcoder configurations are equal; otherwise, false. 22 | * 23 | * @param a first CANcoder configuration to compare 24 | * @param b second CANcoder configuration to compare 25 | * @return true if the two specified CANcoder configurations are equal; otherwise, false 26 | */ 27 | public static boolean isEqual(CANcoderConfiguration a, CANcoderConfiguration b) { 28 | return isEqual(a.MagnetSensor, b.MagnetSensor); 29 | } 30 | 31 | /** 32 | * Returns true if the two specified magnet sensor configurations are equal; otherwise, false. 33 | * 34 | * @param a first magnet sensor configuration to compare 35 | * @param b second magnet sensor configuration to compare 36 | * @return true if the two specified magnet sensor configurations are equal; otherwise, false 37 | */ 38 | public static boolean isEqual(MagnetSensorConfigs a, MagnetSensorConfigs b) { 39 | return MathUtil.isNear( 40 | a.AbsoluteSensorDiscontinuityPoint, 41 | b.AbsoluteSensorDiscontinuityPoint, 42 | PHOENIX_CONFIG_EPSILON) 43 | && MathUtil.isNear(a.MagnetOffset, b.MagnetOffset, PHOENIX_CONFIG_EPSILON) 44 | && a.SensorDirection == b.SensorDirection; 45 | } 46 | 47 | private CANcoderConfigEquality() {} 48 | } 49 | -------------------------------------------------------------------------------- /src/main/deploy/pathplanner/paths/OvalTestFast1.path: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2025.0", 3 | "waypoints": [ 4 | { 5 | "anchor": { 6 | "x": 1.0, 7 | "y": 2.0 8 | }, 9 | "prevControl": null, 10 | "nextControl": { 11 | "x": 1.819152044288992, 12 | "y": 2.573576436351046 13 | }, 14 | "isLocked": false, 15 | "linkedName": null 16 | }, 17 | { 18 | "anchor": { 19 | "x": 3.0, 20 | "y": 3.0 21 | }, 22 | "prevControl": { 23 | "x": 2.25, 24 | "y": 3.0 25 | }, 26 | "nextControl": { 27 | "x": 3.75, 28 | "y": 3.0 29 | }, 30 | "isLocked": false, 31 | "linkedName": null 32 | }, 33 | { 34 | "anchor": { 35 | "x": 5.0, 36 | "y": 2.0 37 | }, 38 | "prevControl": { 39 | "x": 4.196257919204226, 40 | "y": 2.5949778714860567 41 | }, 42 | "nextControl": null, 43 | "isLocked": false, 44 | "linkedName": null 45 | } 46 | ], 47 | "rotationTargets": [], 48 | "constraintZones": [], 49 | "pointTowardsZones": [], 50 | "eventMarkers": [ 51 | { 52 | "name": "ZoneMarker", 53 | "waypointRelativePos": 0.5, 54 | "endWaypointRelativePos": 1.5, 55 | "command": null 56 | }, 57 | { 58 | "name": "Marker", 59 | "waypointRelativePos": 1.0, 60 | "endWaypointRelativePos": null, 61 | "command": null 62 | } 63 | ], 64 | "globalConstraints": { 65 | "maxVelocity": 2.5, 66 | "maxAcceleration": 3.0, 67 | "maxAngularVelocity": 561.96, 68 | "maxAngularAcceleration": 2066.2, 69 | "nominalVoltage": 12.0, 70 | "unlimited": false 71 | }, 72 | "goalEndState": { 73 | "velocity": 0, 74 | "rotation": 180.0 75 | }, 76 | "reversed": false, 77 | "folder": "Tuning", 78 | "idealStartingState": { 79 | "velocity": 0, 80 | "rotation": 0.0 81 | }, 82 | "useDefaultConstraints": false 83 | } -------------------------------------------------------------------------------- /vendordeps/REVLib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "REVLib.json", 3 | "name": "REVLib", 4 | "version": "2025.0.3", 5 | "frcYear": "2025", 6 | "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", 7 | "mavenUrls": [ 8 | "https://maven.revrobotics.com/" 9 | ], 10 | "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", 11 | "javaDependencies": [ 12 | { 13 | "groupId": "com.revrobotics.frc", 14 | "artifactId": "REVLib-java", 15 | "version": "2025.0.3" 16 | } 17 | ], 18 | "jniDependencies": [ 19 | { 20 | "groupId": "com.revrobotics.frc", 21 | "artifactId": "REVLib-driver", 22 | "version": "2025.0.3", 23 | "skipInvalidPlatforms": true, 24 | "isJar": false, 25 | "validPlatforms": [ 26 | "windowsx86-64", 27 | "linuxarm64", 28 | "linuxx86-64", 29 | "linuxathena", 30 | "linuxarm32", 31 | "osxuniversal" 32 | ] 33 | } 34 | ], 35 | "cppDependencies": [ 36 | { 37 | "groupId": "com.revrobotics.frc", 38 | "artifactId": "REVLib-cpp", 39 | "version": "2025.0.3", 40 | "libName": "REVLib", 41 | "headerClassifier": "headers", 42 | "sharedLibrary": false, 43 | "skipInvalidPlatforms": true, 44 | "binaryPlatforms": [ 45 | "windowsx86-64", 46 | "linuxarm64", 47 | "linuxx86-64", 48 | "linuxathena", 49 | "linuxarm32", 50 | "osxuniversal" 51 | ] 52 | }, 53 | { 54 | "groupId": "com.revrobotics.frc", 55 | "artifactId": "REVLib-driver", 56 | "version": "2025.0.3", 57 | "libName": "REVLibDriver", 58 | "headerClassifier": "headers", 59 | "sharedLibrary": false, 60 | "skipInvalidPlatforms": true, 61 | "binaryPlatforms": [ 62 | "windowsx86-64", 63 | "linuxarm64", 64 | "linuxx86-64", 65 | "linuxathena", 66 | "linuxarm32", 67 | "osxuniversal" 68 | ] 69 | } 70 | ] 71 | } 72 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.elevator; 2 | 3 | import edu.wpi.first.units.measure.Distance; 4 | import org.littletonrobotics.junction.AutoLog; 5 | 6 | /** Generic subsystem hardware interface. */ 7 | public interface ElevatorIO { 8 | 9 | // The inputs class for a subsystem usually contains the stator and supply currents, temperature, 10 | // voltage (or current), and, depending on the control mode, additional fields related to position 11 | // or velocity (both measured and reference for each device. The first property is always 12 | // `connected` and logs if each device is reachable. Due to logging limitations, properties cannot 13 | // be a subtype of Measure. Therefore all properties are suffix with their unit to mitigate bugs 14 | // due to unit mismatches. 15 | @AutoLog 16 | public static class ElevatorIOInputs { 17 | 18 | boolean connectedLead = false; 19 | boolean connectedFollower = false; 20 | 21 | double voltageSuppliedLead = 0.0; 22 | double voltageSuppliedFollower = 0.0; 23 | 24 | double statorCurrentAmpsLead = 0.0; 25 | double statorCurrentAmpsFollower = 0.0; 26 | 27 | double supplyCurrentAmpsLead = 0.0; 28 | double supplyCurrentAmpsFollower = 0.0; 29 | 30 | double velocityRPS = 0.0; 31 | 32 | double closedLoopError = 0.0; 33 | double closedLoopReference = 0.0; 34 | 35 | double positionInches = 0.0; 36 | double positionRotations = 0.0; 37 | 38 | double leadTempCelsius = 0.0; 39 | double followerTempCelsius = 0.0; 40 | } 41 | 42 | /** Updates the set of loggable inputs. */ 43 | public default void updateInputs(ElevatorIOInputs inputs) {} 44 | 45 | /** 46 | * Set the motor position to the specified value in degrees. 47 | * 48 | * @param position the position to set the motor to in degrees 49 | */ 50 | public default void setPosition(Distance position) {} 51 | 52 | public default void zeroPosition() {} 53 | 54 | public default void setMotorVoltage(double volts) {} 55 | } 56 | -------------------------------------------------------------------------------- /vendordeps/photonlib.json: -------------------------------------------------------------------------------- 1 | { 2 | "fileName": "photonlib.json", 3 | "name": "photonlib", 4 | "version": "v2025.2.1", 5 | "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", 6 | "frcYear": "2025", 7 | "mavenUrls": [ 8 | "https://maven.photonvision.org/repository/internal", 9 | "https://maven.photonvision.org/repository/snapshots" 10 | ], 11 | "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", 12 | "jniDependencies": [ 13 | { 14 | "groupId": "org.photonvision", 15 | "artifactId": "photontargeting-cpp", 16 | "version": "v2025.2.1", 17 | "skipInvalidPlatforms": true, 18 | "isJar": false, 19 | "validPlatforms": [ 20 | "windowsx86-64", 21 | "linuxathena", 22 | "linuxx86-64", 23 | "osxuniversal" 24 | ] 25 | } 26 | ], 27 | "cppDependencies": [ 28 | { 29 | "groupId": "org.photonvision", 30 | "artifactId": "photonlib-cpp", 31 | "version": "v2025.2.1", 32 | "libName": "photonlib", 33 | "headerClassifier": "headers", 34 | "sharedLibrary": true, 35 | "skipInvalidPlatforms": true, 36 | "binaryPlatforms": [ 37 | "windowsx86-64", 38 | "linuxathena", 39 | "linuxx86-64", 40 | "osxuniversal" 41 | ] 42 | }, 43 | { 44 | "groupId": "org.photonvision", 45 | "artifactId": "photontargeting-cpp", 46 | "version": "v2025.2.1", 47 | "libName": "photontargeting", 48 | "headerClassifier": "headers", 49 | "sharedLibrary": true, 50 | "skipInvalidPlatforms": true, 51 | "binaryPlatforms": [ 52 | "windowsx86-64", 53 | "linuxathena", 54 | "linuxx86-64", 55 | "osxuniversal" 56 | ] 57 | } 58 | ], 59 | "javaDependencies": [ 60 | { 61 | "groupId": "org.photonvision", 62 | "artifactId": "photonlib-java", 63 | "version": "v2025.2.1" 64 | }, 65 | { 66 | "groupId": "org.photonvision", 67 | "artifactId": "photontargeting-java", 68 | "version": "v2025.2.1" 69 | } 70 | ] 71 | } 72 | -------------------------------------------------------------------------------- /simgui-ds.json: -------------------------------------------------------------------------------- 1 | { 2 | "keyboardJoysticks": [ 3 | { 4 | "axisConfig": [ 5 | { 6 | "decKey": 65, 7 | "incKey": 68 8 | }, 9 | { 10 | "decKey": 87, 11 | "incKey": 83 12 | }, 13 | { 14 | "decKey": 69, 15 | "decayRate": 0.0, 16 | "incKey": 82, 17 | "keyRate": 0.009999999776482582 18 | } 19 | ], 20 | "axisCount": 3, 21 | "buttonCount": 4, 22 | "buttonKeys": [ 23 | 90, 24 | 88, 25 | 67, 26 | 86 27 | ], 28 | "povConfig": [ 29 | { 30 | "key0": 328, 31 | "key135": 323, 32 | "key180": 322, 33 | "key225": 321, 34 | "key270": 324, 35 | "key315": 327, 36 | "key45": 329, 37 | "key90": 326 38 | } 39 | ], 40 | "povCount": 1 41 | }, 42 | { 43 | "axisConfig": [ 44 | { 45 | "decKey": 74, 46 | "incKey": 76 47 | }, 48 | { 49 | "decKey": 73, 50 | "incKey": 75 51 | } 52 | ], 53 | "axisCount": 2, 54 | "buttonCount": 4, 55 | "buttonKeys": [ 56 | 77, 57 | 44, 58 | 46, 59 | 47 60 | ], 61 | "povCount": 0 62 | }, 63 | { 64 | "axisConfig": [ 65 | { 66 | "decKey": 263, 67 | "incKey": 262 68 | }, 69 | { 70 | "decKey": 265, 71 | "incKey": 264 72 | } 73 | ], 74 | "axisCount": 2, 75 | "buttonCount": 6, 76 | "buttonKeys": [ 77 | 260, 78 | 268, 79 | 266, 80 | 261, 81 | 269, 82 | 267 83 | ], 84 | "povCount": 0 85 | }, 86 | { 87 | "axisCount": 0, 88 | "buttonCount": 0, 89 | "povCount": 0 90 | } 91 | ], 92 | "robotJoysticks": [ 93 | { 94 | "guid": "Keyboard0" 95 | }, 96 | { 97 | "guid": "Keyboard1" 98 | } 99 | ] 100 | } 101 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/swerve_drivetrain/SysIdSwerveSteerGains_Torque.java: -------------------------------------------------------------------------------- 1 | // from Team 5712 2 | 3 | package frc.lib.team3061.swerve_drivetrain; 4 | 5 | import static edu.wpi.first.units.Units.*; 6 | 7 | import com.ctre.phoenix6.StatusCode; 8 | import com.ctre.phoenix6.controls.CoastOut; 9 | import com.ctre.phoenix6.controls.TorqueCurrentFOC; 10 | import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters; 11 | import com.ctre.phoenix6.swerve.SwerveModule; 12 | import com.ctre.phoenix6.swerve.SwerveRequest; 13 | import edu.wpi.first.units.measure.*; 14 | 15 | /** 16 | * SysId-specific SwerveRequest to characterize the steer module characteristics of a swerve 17 | * drivetrain. 18 | */ 19 | public class SysIdSwerveSteerGains_Torque implements SwerveRequest { 20 | /** Torque current to apply to steer motors. */ 21 | public double TorqueCurrentToApply = 0; 22 | 23 | /* Local reference to a coast request for the drive motors */ 24 | private final CoastOut m_driveRequest = new CoastOut(); 25 | /* Local reference to a torque current request for the steer motors */ 26 | private final TorqueCurrentFOC m_steerRequest = new TorqueCurrentFOC(0); 27 | 28 | public StatusCode apply( 29 | SwerveControlParameters parameters, SwerveModule... modulesToApply) { 30 | for (int i = 0; i < modulesToApply.length; ++i) { 31 | modulesToApply[i].apply(m_driveRequest, m_steerRequest.withOutput(TorqueCurrentToApply)); 32 | } 33 | return StatusCode.OK; 34 | } 35 | 36 | /** 37 | * Sets the torque current to apply to the drive wheels. 38 | * 39 | * @param current Torque current to apply 40 | * @return this request 41 | */ 42 | public SysIdSwerveSteerGains_Torque withTorqueCurrent(double current) { 43 | TorqueCurrentToApply = current; 44 | return this; 45 | } 46 | /** 47 | * Sets the torque current to apply to the drive wheels. 48 | * 49 | * @param current Torque current to apply 50 | * @return this request 51 | */ 52 | public SysIdSwerveSteerGains_Torque withTorqueCurrent(Current current) { 53 | TorqueCurrentToApply = current.in(Amps); 54 | return this; 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/leds/LEDsCANdle.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.leds; 2 | 3 | import com.ctre.phoenix.led.CANdle; 4 | import com.ctre.phoenix.led.CANdle.LEDStripType; 5 | import com.ctre.phoenix.led.CANdle.VBatOutputMode; 6 | import com.ctre.phoenix.led.CANdleConfiguration; 7 | import edu.wpi.first.wpilibj.util.Color; 8 | import edu.wpi.first.wpilibj.util.Color8Bit; 9 | import frc.lib.team3061.RobotConfig; 10 | 11 | public class LEDsCANdle extends LEDs { 12 | private final CANdle candle; 13 | private final Color8Bit[] ledBuffer; 14 | 15 | protected LEDsCANdle() { 16 | 17 | candle = new CANdle(22, RobotConfig.getInstance().getCANBusName()); 18 | 19 | CANdleConfiguration configSettings = new CANdleConfiguration(); 20 | configSettings.statusLedOffWhenActive = true; 21 | configSettings.disableWhenLOS = false; 22 | configSettings.stripType = LEDStripType.GRB; 23 | configSettings.brightnessScalar = .1; 24 | configSettings.vBatOutputMode = VBatOutputMode.Modulated; 25 | candle.configAllSettings(configSettings, 100); 26 | 27 | ledBuffer = new Color8Bit[ACTUAL_LENGTH]; 28 | for (int i = 0; i < ACTUAL_LENGTH; i++) { 29 | ledBuffer[i] = convertTo8BitColor(Color.kBlack); 30 | } 31 | } 32 | 33 | @Override 34 | protected void updateLEDs() { 35 | for (int i = 0; i < ACTUAL_LENGTH; i++) { 36 | int count = 1; 37 | while (i + count < ACTUAL_LENGTH && ledBuffer[i] == ledBuffer[i + count]) { 38 | count++; 39 | } 40 | candle.setLEDs(ledBuffer[i].red, ledBuffer[i].green, ledBuffer[i].blue, 0, i, count); 41 | } 42 | } 43 | 44 | @Override 45 | protected void setLEDBuffer(int index, Color color) { 46 | ledBuffer[index] = convertTo8BitColor(color); 47 | if (MIRROR_LEDS) { 48 | ledBuffer[ACTUAL_LENGTH - index - 1] = convertTo8BitColor(color); 49 | } 50 | } 51 | 52 | @Override 53 | public Color8Bit getColor(int index) { 54 | return ledBuffer[index]; 55 | } 56 | 57 | private static Color8Bit convertTo8BitColor(Color color) { 58 | return new Color8Bit( 59 | (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255)); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team6328/util/NTClientLogger.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 FRC 6328 2 | // http://github.com/Mechanical-Advantage 3 | // 4 | // Use of this source code is governed by an MIT-style 5 | // license that can be found in the LICENSE file at 6 | // the root directory of this project. 7 | 8 | package frc.lib.team6328.util; 9 | 10 | import edu.wpi.first.networktables.ConnectionInfo; 11 | import edu.wpi.first.networktables.NetworkTableInstance; 12 | import java.nio.ByteBuffer; 13 | import java.util.HashSet; 14 | import java.util.Set; 15 | import org.littletonrobotics.junction.Logger; 16 | 17 | /** Utility class to log the list of NetworkTables clients. */ 18 | public class NTClientLogger { 19 | private static final String tableName = "NTClients/"; 20 | private static Set lastRemoteIds = new HashSet<>(); 21 | private static ByteBuffer intBuffer = ByteBuffer.allocate(4); 22 | 23 | private NTClientLogger() {} 24 | 25 | public static void log() { 26 | ConnectionInfo[] connections = NetworkTableInstance.getDefault().getConnections(); 27 | Set remoteIds = new HashSet<>(); 28 | 29 | // Log data for connected clients 30 | for (int i = 0; i < connections.length; i++) { 31 | lastRemoteIds.remove(connections[i].remote_id); 32 | remoteIds.add(connections[i].remote_id); 33 | Logger.recordOutput(tableName + connections[i].remote_id + "/Connected", true); 34 | Logger.recordOutput( 35 | tableName + connections[i].remote_id + "/IPAddress", connections[i].remote_ip); 36 | Logger.recordOutput( 37 | tableName + connections[i].remote_id + "/RemotePort", connections[i].remote_port); 38 | Logger.recordOutput( 39 | tableName + connections[i].remote_id + "/LastUpdate", connections[i].last_update); 40 | intBuffer.rewind(); 41 | Logger.recordOutput( 42 | tableName + connections[i].remote_id + "/ProtocolVersion", 43 | intBuffer.putInt(connections[i].protocol_version).array()); 44 | } 45 | 46 | // Mark disconnected clients 47 | for (var remoteId : lastRemoteIds) { 48 | Logger.recordOutput(tableName + remoteId + "/Connected", false); 49 | } 50 | lastRemoteIds = remoteIds; 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/arm/ArmConstants.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.arm; 2 | 3 | public class ArmConstants { 4 | 5 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 6 | 7 | private ArmConstants() { 8 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 9 | } 10 | 11 | public static final String SUBSYSTEM_NAME = "Arm"; 12 | 13 | public static final int ANGLE_MOTOR_ID = 29; 14 | public static final int ANGLE_ENCODER_ID = 9; 15 | 16 | // PID constants are determined empirically through tuning 17 | public static final double ROTATION_KP = 50; 18 | public static final double ROTATION_KI = 0.0; 19 | public static final double ROTATION_KD = 10; 20 | 21 | // feed forward constants are determined through running SysId commands and analyzing the results 22 | // in SysId 23 | public static final double ROTATION_KS = 0.16206; 24 | public static final double ROTATION_KG = 0.22544; 25 | public static final double ROTATION_KV = 20.835; 26 | public static final double ROTATION_KA = 0.16507; 27 | 28 | // Motion magic constants are determined empirically through tuning 29 | public static final double ROTATION_EXPO_KV = 80.0; 30 | public static final double ROTATION_EXPO_KA = 24.0; 31 | public static final double MOTION_MAGIC_CRUISE_VELOCITY = 0; // don't limit the cruise velocity 32 | 33 | // current limits are determined based on current budget for the robot 34 | public static final double ANGLE_MOTOR_CONTINUOUS_CURRENT_LIMIT = 15; 35 | public static final double ANGLE_MOTOR_PEAK_CURRENT_LIMIT = 20; 36 | public static final double ANGLE_MOTOR_PEAK_CURRENT_DURATION = 0.1; 37 | 38 | // the following are determined based on the mechanical design of the arm 39 | public static final boolean ANGLE_MOTOR_INVERTED = true; 40 | public static final double ANGLE_MOTOR_GEAR_RATIO = 45.0; 41 | public static final double SENSOR_TO_MECHANISM_RATIO = 4.0; 42 | public static final double MAGNET_OFFSET = -0.70727; 43 | public static final double LOWER_ANGLE_LIMIT = 14.7; 44 | public static final double UPPER_ANGLE_LIMIT = 135.0; 45 | public static final double ANGLE_TOLERANCE_DEGREES = 1.5; 46 | 47 | public static final double ANGLE_MOTOR_MANUAL_CONTROL_VOLTAGE = 1.0; 48 | } 49 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.shooter; 2 | 3 | import static edu.wpi.first.units.Units.RotationsPerSecond; 4 | 5 | import edu.wpi.first.units.measure.AngularVelocity; 6 | 7 | public class ShooterConstants { 8 | 9 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 10 | 11 | private ShooterConstants() { 12 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 13 | } 14 | 15 | public static final String SUBSYSTEM_NAME = "Shooter"; 16 | 17 | public static final int TOP_SHOOTER_MOTOR_ID = 27; 18 | public static final int BOTTOM_SHOOTER_MOTOR_ID = 28; 19 | 20 | // PID constants are determined empirically through tuning 21 | public static final double TOP_SHOOT_KP = 5.0; 22 | public static final double TOP_SHOOT_KI = 0.0; 23 | public static final double TOP_SHOOT_KD = 0.0; 24 | public static final double BOTTOM_SHOOT_KP = 5.0; 25 | public static final double BOTTOM_SHOOT_KI = 0.0; 26 | public static final double BOTTOM_SHOOT_KD = 0.0; 27 | 28 | // feed forward constants are determined through running SysId commands and analyzing the results 29 | // in SysId 30 | public static final double TOP_SHOOT_KS = 6.0; 31 | public static final double BOTTOM_SHOOT_KS = 7.0; 32 | 33 | // current limits are determined based on current budget for the robot 34 | public static final double SHOOT_MOTOR_TOP_CONTINUOUS_CURRENT_LIMIT = 30; 35 | public static final double SHOOT_MOTOR_TOP_PEAK_CURRENT_LIMIT = 60; 36 | public static final double SHOOT_MOTOR_TOP_PEAK_CURRENT_DURATION = 0.1; 37 | public static final double SHOOT_MOTOR_BOTTOM_CONTINUOUS_CURRENT_LIMIT = 30; 38 | public static final double SHOOT_MOTOR_BOTTOM_PEAK_CURRENT_LIMIT = 60; 39 | public static final double SHOOT_MOTOR_BOTTOM_PEAK_CURRENT_DURATION = 0.1; 40 | 41 | // the following are determined based on the mechanical design of the shooter 42 | public static final double SHOOT_MOTORS_GEAR_RATIO = 0.5; 43 | public static final boolean SHOOT_TOP_INVERTED = false; 44 | public static final boolean SHOOT_BOTTOM_INVERTED = false; 45 | public static final AngularVelocity SHOOTER_IDLE_VELOCITY = RotationsPerSecond.of(10.0); 46 | public static final double VELOCITY_TOLERANCE_RPS = 5.0; 47 | } 48 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/SingleHandheldOI.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.operator_interface; 6 | 7 | import edu.wpi.first.wpilibj2.command.button.CommandXboxController; 8 | import edu.wpi.first.wpilibj2.command.button.Trigger; 9 | 10 | /** Class for controlling the robot with a single Xbox controller. */ 11 | public class SingleHandheldOI extends OperatorDashboard { 12 | private final CommandXboxController controller; 13 | 14 | public SingleHandheldOI(int port) { 15 | controller = new CommandXboxController(port); 16 | } 17 | 18 | @Override 19 | public double getTranslateX() { 20 | return -controller.getLeftY(); 21 | } 22 | 23 | @Override 24 | public double getTranslateY() { 25 | return -controller.getLeftX(); 26 | } 27 | 28 | @Override 29 | public double getRotate() { 30 | return -controller.getRightX(); 31 | } 32 | 33 | @Override 34 | public Trigger getFieldRelativeButton() { 35 | return controller.b(); 36 | } 37 | 38 | @Override 39 | public Trigger getResetGyroButton() { 40 | return controller.start(); 41 | } 42 | 43 | @Override 44 | public Trigger getXStanceButton() { 45 | return controller.y(); 46 | } 47 | 48 | @Override 49 | public Trigger getTranslationSlowModeButton() { 50 | return controller.leftBumper(); 51 | } 52 | 53 | @Override 54 | public Trigger getRotationSlowModeButton() { 55 | return controller.rightBumper(); 56 | } 57 | 58 | @Override 59 | public Trigger getLock180Button() { 60 | return controller.a(); 61 | } 62 | 63 | @Override 64 | public Trigger getSysIdDynamicForward() { 65 | return controller.back().and(controller.y()); 66 | } 67 | 68 | @Override 69 | public Trigger getSysIdDynamicReverse() { 70 | return controller.back().and(controller.x()); 71 | } 72 | 73 | @Override 74 | public Trigger getSysIdQuasistaticForward() { 75 | return controller.start().and(controller.y()); 76 | } 77 | 78 | @Override 79 | public Trigger getSysIdQuasistaticReverse() { 80 | return controller.start().and(controller.x()); 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/sim/VelocitySystemSim.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.sim; 2 | 3 | import com.ctre.phoenix6.hardware.TalonFX; 4 | import com.ctre.phoenix6.sim.ChassisReference; 5 | import com.ctre.phoenix6.sim.TalonFXSimState; 6 | import edu.wpi.first.math.numbers.N1; 7 | import edu.wpi.first.math.system.plant.LinearSystemId; 8 | import edu.wpi.first.wpilibj.RobotController; 9 | import edu.wpi.first.wpilibj.simulation.LinearSystemSim; 10 | import frc.robot.Constants; 11 | 12 | public class VelocitySystemSim { 13 | 14 | private TalonFX motor; 15 | private TalonFXSimState motorSimState; 16 | private LinearSystemSim systemSim; 17 | private double gearRatio; 18 | 19 | public VelocitySystemSim( 20 | TalonFX motor, boolean motorInverted, double kV, double kA, double gearRatio) { 21 | this.motor = motor; 22 | this.gearRatio = gearRatio; 23 | 24 | if (Constants.getMode() != Constants.Mode.SIM) { 25 | return; 26 | } 27 | 28 | this.motorSimState = this.motor.getSimState(); 29 | this.motorSimState.Orientation = 30 | motorInverted 31 | ? ChassisReference.Clockwise_Positive 32 | : ChassisReference.CounterClockwise_Positive; 33 | 34 | this.systemSim = new LinearSystemSim<>(LinearSystemId.identifyVelocitySystem(kV, kA)); 35 | } 36 | 37 | public void updateSim() { 38 | if (Constants.getMode() != Constants.Mode.SIM) { 39 | return; 40 | } 41 | 42 | // update the sim states supply voltage based on the simulated battery 43 | this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); 44 | 45 | // update the input voltages of the models based on the outputs of the simulated TalonFXs 46 | this.systemSim.setInput(this.motorSimState.getMotorVoltage()); 47 | 48 | // update the models 49 | this.systemSim.update(Constants.LOOP_PERIOD_SECS); 50 | 51 | // update the simulated TalonFX based on the model outputs 52 | double mechanismRadiansPerSec = this.systemSim.getOutput(0); 53 | double motorRPS = mechanismRadiansPerSec * this.gearRatio / (2 * Math.PI); 54 | double motorRotations = motorRPS * Constants.LOOP_PERIOD_SECS; 55 | this.motorSimState.addRotorPosition(motorRotations); 56 | this.motorSimState.setRotorVelocity(motorRPS); 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/shooter/ShooterIO.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.shooter; 2 | 3 | import edu.wpi.first.units.measure.AngularVelocity; 4 | import edu.wpi.first.units.measure.Current; 5 | import org.littletonrobotics.junction.AutoLog; 6 | 7 | public interface ShooterIO { 8 | 9 | // The inputs class for a subsystem usually contains the stator and supply currents, temperature, 10 | // voltage (or current), and, depending on the control mode, additional fields related to position 11 | // or velocity (both measured and reference). The first property is always `connected` and logs if 12 | // each device is reachable. Due to logging limitations, properties cannot be a subtype of 13 | // Measure. Therefore all properties are suffix with their unit to mitigate bugs due to unit 14 | // mismatches. 15 | @AutoLog 16 | public static class ShooterIOInputs { 17 | 18 | // Top Shooter Motor Inputs 19 | boolean shootMotorTopConnected = false; 20 | double shootMotorTopStatorCurrentAmps = 0.0; 21 | double shootMotorTopSupplyCurrentAmps = 0.0; 22 | double shootMotorTopVelocityRPS = 0.0; 23 | double shootMotorTopReferenceVelocityRPS = 0.0; 24 | double shootMotorTopClosedLoopReferenceVelocityRPS = 0.0; 25 | double shootMotorTopClosedLoopErrorVelocityRPS = 0.0; 26 | double shootMotorTopTemperatureCelsius = 0.0; 27 | double shootMotorTopVoltage = 0.0; 28 | 29 | // Bottom Shooter Motor Inputs 30 | boolean shootMotorBottomConnected = false; 31 | double shootMotorBottomStatorCurrentAmps = 0.0; 32 | double shootMotorBottomSupplyCurrentAmps = 0.0; 33 | double shootMotorBottomVelocityRPS = 0.0; 34 | double shootMotorBottomReferenceVelocityRPS = 0.0; 35 | double shootMotorBottomClosedLoopReferenceVelocityRPS = 0.0; 36 | double shootMotorBottomClosedLoopErrorVelocityRPS = 0.0; 37 | double shootMotorBottomTemperatureCelsius = 0.0; 38 | double shootMotorBottomVoltage = 0.0; 39 | } 40 | 41 | public default void updateInputs(ShooterIOInputs inputs) {} 42 | 43 | public default void setShooterWheelTopVelocity(AngularVelocity velocity) {} 44 | 45 | public default void setShooterWheelBottomVelocity(AngularVelocity velocity) {} 46 | 47 | public default void setShooterWheelTopCurrent(Current amps) {} 48 | 49 | public default void setShooterWheelBottomCurrent(Current amps) {} 50 | } 51 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/commands/ArcadeDrive.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.commands; 6 | 7 | import edu.wpi.first.wpilibj2.command.Command; 8 | import frc.lib.team3061.differential_drivetrain.DifferentialDrivetrain; 9 | import java.util.function.Supplier; 10 | import org.littletonrobotics.junction.Logger; 11 | 12 | public class ArcadeDrive extends Command { 13 | private final DifferentialDrivetrain drivetrain; 14 | private final Supplier xVelocitySupplier; 15 | private final Supplier rotationVelocitySupplier; 16 | 17 | /** 18 | * Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier 19 | * lambdas. This command does not terminate. 20 | * 21 | * @param drivetrain The drivetrain subsystem on which this command will run 22 | * @param xVelocitySupplier Lambda supplier of forward/backward speed 23 | * @param rotationVelocitySupplier Lambda supplier of rotational speed 24 | */ 25 | public ArcadeDrive( 26 | DifferentialDrivetrain drivetrain, 27 | Supplier xVelocitySupplier, 28 | Supplier rotationVelocitySupplier) { 29 | this.drivetrain = drivetrain; 30 | this.xVelocitySupplier = xVelocitySupplier; 31 | this.rotationVelocitySupplier = rotationVelocitySupplier; 32 | addRequirements(drivetrain); 33 | } 34 | 35 | // Called when the command is initially scheduled. 36 | @Override 37 | public void initialize() {} 38 | 39 | // Called every time the scheduler runs while the command is scheduled. 40 | @Override 41 | public void execute() { 42 | 43 | double xVelocity = xVelocitySupplier.get(); 44 | double rotationalVelocity = rotationVelocitySupplier.get(); 45 | 46 | drivetrain.arcadeDrive(xVelocity, rotationalVelocity); 47 | 48 | Logger.recordOutput("ArcadeDrive/xVelocity", xVelocity); 49 | Logger.recordOutput("ArcadeDrive/rotationalVelocity", rotationalVelocity); 50 | } 51 | 52 | // Called once the command ends or is interrupted. 53 | @Override 54 | public void end(boolean interrupted) {} 55 | 56 | // Returns true when the command should end. 57 | @Override 58 | public boolean isFinished() { 59 | return false; 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/swerve_drivetrain/SysIdSwerveTranslation_Torque.java: -------------------------------------------------------------------------------- 1 | // from Team 5712 2 | 3 | package frc.lib.team3061.swerve_drivetrain; 4 | 5 | import static edu.wpi.first.units.Units.*; 6 | 7 | import com.ctre.phoenix6.StatusCode; 8 | import com.ctre.phoenix6.controls.PositionVoltage; 9 | import com.ctre.phoenix6.controls.TorqueCurrentFOC; 10 | import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveControlParameters; 11 | import com.ctre.phoenix6.swerve.SwerveModule; 12 | import com.ctre.phoenix6.swerve.SwerveRequest; 13 | import edu.wpi.first.units.measure.*; 14 | 15 | /** 16 | * SysId-specific SwerveRequest to characterize the translational characteristics of a swerve 17 | * drivetrain. 18 | */ 19 | public class SysIdSwerveTranslation_Torque implements SwerveRequest { 20 | /** Torque current to apply to drive wheels. */ 21 | public double TorqueCurrentToApply = 0; 22 | 23 | /* Local reference to a torque current request for the drive motors */ 24 | private final TorqueCurrentFOC m_driveRequest = new TorqueCurrentFOC(0); 25 | /* Local reference to a position request for the steer motors */ 26 | // Select one of the two below based on which type of steer closed-loop you are using 27 | private final PositionVoltage m_steerRequest = new PositionVoltage(0); 28 | // private final PositionTorqueCurrentFOC m_steerRequest = new PositionTorqueCurrentFOC(0); 29 | 30 | public StatusCode apply( 31 | SwerveControlParameters parameters, SwerveModule... modulesToApply) { 32 | for (int i = 0; i < modulesToApply.length; ++i) { 33 | modulesToApply[i].apply( 34 | m_driveRequest.withOutput(TorqueCurrentToApply), m_steerRequest.withPosition(0)); 35 | } 36 | return StatusCode.OK; 37 | } 38 | 39 | /** 40 | * Sets the torque current to apply to the drive wheels. 41 | * 42 | * @param current Torque current to apply 43 | * @return this request 44 | */ 45 | public SysIdSwerveTranslation_Torque withTorqueCurrent(double current) { 46 | TorqueCurrentToApply = current; 47 | return this; 48 | } 49 | /** 50 | * Sets the torque current to apply to the drive wheels. 51 | * 52 | * @param current Torque current to apply 53 | * @return this request 54 | */ 55 | public SysIdSwerveTranslation_Torque withTorqueCurrent(Current current) { 56 | TorqueCurrentToApply = current.in(Amps); 57 | return this; 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/util/GeometryUtils.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.util; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Translation2d; 6 | import edu.wpi.first.math.geometry.Twist2d; 7 | 8 | public class GeometryUtils { 9 | private static final double EPSILON = 1E-9; 10 | 11 | private GeometryUtils() {} 12 | 13 | /** 14 | * Obtain a new Pose2d from a (constant curvature) velocity. See: 15 | * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp . Borrowed from 254: 16 | * https://github.com/Team254/FRC-2022-Public/blob/b5da3c760b78d598b492e1cc51d8331c2ad50f6a/src/main/java/com/team254/lib/geometry/Pose2d.java 17 | */ 18 | public static Pose2d exp(final Twist2d delta) { 19 | double sinTheta = Math.sin(delta.dtheta); 20 | double cosTheta = Math.cos(delta.dtheta); 21 | double s; 22 | double c; 23 | if (Math.abs(delta.dtheta) < EPSILON) { 24 | s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta; 25 | c = .5 * delta.dtheta; 26 | } else { 27 | s = sinTheta / delta.dtheta; 28 | c = (1.0 - cosTheta) / delta.dtheta; 29 | } 30 | return new Pose2d( 31 | new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), 32 | new Rotation2d(cosTheta, sinTheta)); 33 | } 34 | 35 | /** 36 | * Logical inverse of the above. Borrowed from 254: 37 | * https://github.com/Team254/FRC-2022-Public/blob/b5da3c760b78d598b492e1cc51d8331c2ad50f6a/src/main/java/com/team254/lib/geometry/Pose2d.java 38 | */ 39 | public static Twist2d log(final Pose2d transform) { 40 | final double dtheta = transform.getRotation().getRadians(); 41 | final double half_dtheta = 0.5 * dtheta; 42 | final double cos_minus_one = Math.cos(transform.getRotation().getRadians()) - 1.0; 43 | double halfThetaByTanOfHalfDtheta; 44 | if (Math.abs(cos_minus_one) < EPSILON) { 45 | halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; 46 | } else { 47 | halfThetaByTanOfHalfDtheta = 48 | -(half_dtheta * Math.sin(transform.getRotation().getRadians())) / cos_minus_one; 49 | } 50 | final Translation2d translationPart = 51 | transform 52 | .getTranslation() 53 | .rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -half_dtheta)); 54 | return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /simgui.json: -------------------------------------------------------------------------------- 1 | { 2 | "HALProvider": { 3 | "Addressable LEDs": { 4 | "0": { 5 | "columns": 35 6 | }, 7 | "window": { 8 | "visible": true 9 | } 10 | }, 11 | "DIO": { 12 | "window": { 13 | "visible": true 14 | } 15 | }, 16 | "Other Devices": { 17 | "BuiltInAccel": { 18 | "header": { 19 | "open": true 20 | } 21 | }, 22 | "XRPGyro": { 23 | "header": { 24 | "open": true 25 | } 26 | } 27 | } 28 | }, 29 | "NTProvider": { 30 | "types": { 31 | "/AdvantageKit/RealOutputs/Alerts": "Alerts", 32 | "/AdvantageKit/RealOutputs/Arm/ArmSim": "Mechanism2d", 33 | "/AdvantageKit/RealOutputs/Elevator/ElevatorSim": "Mechanism2d", 34 | "/AdvantageKit/RealOutputs/PathPlanner": "Alerts", 35 | "/AdvantageKit/RealOutputs/Visualization/Mechanisms2D": "Mechanism2d", 36 | "/FMSInfo": "FMSInfo", 37 | "/SmartDashboard/Alerts": "Alerts", 38 | "/SmartDashboard/Auto Routine": "String Chooser", 39 | "/SmartDashboard/PathPlanner": "Alerts", 40 | "/SmartDashboard/SysId Chooser": "String Chooser", 41 | "/SmartDashboard/SystemStatus/Arm/SystemCheck": "Command", 42 | "/SmartDashboard/SystemStatus/CheckForFaults": "Command", 43 | "/SmartDashboard/SystemStatus/ClearAllFaults": "Command", 44 | "/SmartDashboard/SystemStatus/Drivetrain/SystemCheck": "Command", 45 | "/SmartDashboard/SystemStatus/Elevator/SystemCheck": "Command", 46 | "/SmartDashboard/SystemStatus/Manipulator/SystemCheck": "Command", 47 | "/SmartDashboard/SystemStatus/Shooter/SystemCheck": "Command", 48 | "/SmartDashboard/SystemStatus/Subsystem/SystemCheck": "Command", 49 | "/SmartDashboard/SystemStatus/SwerveModule0/SystemCheck": "Command", 50 | "/SmartDashboard/SystemStatus/SwerveModule1/SystemCheck": "Command", 51 | "/SmartDashboard/SystemStatus/SwerveModule2/SystemCheck": "Command", 52 | "/SmartDashboard/SystemStatus/SwerveModule3/SystemCheck": "Command", 53 | "/SmartDashboard/VisionSystemSim-OV2311BL/Sim Field": "Field2d", 54 | "/SmartDashboard/VisionSystemSim-OV2311BR/Sim Field": "Field2d", 55 | "/SmartDashboard/VisionSystemSim-OV2311FL/Sim Field": "Field2d", 56 | "/SmartDashboard/VisionSystemSim-OV2311FR/Sim Field": "Field2d", 57 | "/SmartDashboard/VisionSystemSim-simCamera/Sim Field": "Field2d" 58 | } 59 | }, 60 | "NetworkTables Info": { 61 | "visible": true 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/pneumatics/PneumaticsIORev.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.lib.team3061.pneumatics; 10 | 11 | import static frc.robot.Constants.*; 12 | 13 | import edu.wpi.first.wpilibj.AnalogInput; 14 | import edu.wpi.first.wpilibj.PneumaticHub; 15 | import frc.lib.team3061.RobotConfig; 16 | 17 | /** 18 | * Implementation of PneumaticsIO for REV Robotics Pneumatics Hub, dual REV Robotics pressure 19 | * sensors, and an analog flow sensor (i.e., SMC PFM711-N7-C-R.) 20 | */ 21 | public class PneumaticsIORev implements PneumaticsIO { 22 | 23 | private static final int MIN_LOW_PRESSURE = 50; 24 | private static final int MAX_LOW_PRESSURE = 60; 25 | private static final int MIN_HIGH_PRESSURE = 80; 26 | private static final int MAX_HIGH_PRESSURE = 120; 27 | 28 | private final PneumaticHub pneumatics; 29 | private final AnalogInput flowSensor; 30 | 31 | public PneumaticsIORev() { 32 | pneumatics = new PneumaticHub(RobotConfig.getInstance().getPneumaticsHubCANID()); 33 | flowSensor = new AnalogInput(RobotConfig.getInstance().getFlowSensorChannel()); 34 | useLowClosedLoopThresholds(false); 35 | } 36 | 37 | @Override 38 | public void updateInputs(PneumaticsIOInputs inputs) { 39 | inputs.highPressurePSI = 40 | pneumatics.getPressure(RobotConfig.getInstance().getRevHighPressureSensorChannel()); 41 | inputs.lowPressurePSI = 42 | pneumatics.getPressure(RobotConfig.getInstance().getRevLowPressureSensorChannel()); 43 | inputs.compressorActive = pneumatics.getCompressor(); 44 | inputs.compressorCurrentAmps = pneumatics.getCompressorCurrent(); 45 | 46 | /* 47 | * Our SMC flow sensor (PFM711-N7-C-R) provides analog output from 1V to 5V. 48 | * 1V corresponds to 0 L/min; 5V corresponds to 100 L/min. 49 | */ 50 | inputs.flowLPM = ((flowSensor.getAverageVoltage() * 25) - 25); 51 | inputs.volumeL += (inputs.flowLPM * LOOP_PERIOD_SECS) / 60; 52 | } 53 | 54 | @Override 55 | public void useLowClosedLoopThresholds(boolean useLow) { 56 | if (useLow) { 57 | pneumatics.enableCompressorAnalog(MIN_LOW_PRESSURE, MAX_LOW_PRESSURE); 58 | } else { 59 | pneumatics.enableCompressorAnalog(MIN_HIGH_PRESSURE, MAX_HIGH_PRESSURE); 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team254/ChezyRepeatCommand.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team254; 2 | 3 | import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; 4 | 5 | import edu.wpi.first.util.sendable.SendableBuilder; 6 | import edu.wpi.first.wpilibj2.command.Command; 7 | import edu.wpi.first.wpilibj2.command.CommandScheduler; 8 | 9 | public class ChezyRepeatCommand extends Command { 10 | private final Command m_command; 11 | private boolean m_ended; 12 | private final int kMaxLoops = 3; 13 | 14 | /** 15 | * Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it 16 | * ends, until this command is interrupted. 17 | * 18 | * @param command the command to run repeatedly 19 | */ 20 | @SuppressWarnings("this-escape") 21 | public ChezyRepeatCommand(Command command) { 22 | m_command = requireNonNullParam(command, "command", "RepeatCommand"); 23 | CommandScheduler.getInstance().registerComposedCommands(command); 24 | addRequirements(command.getRequirements()); 25 | setName("Repeat(" + command.getName() + ")"); 26 | } 27 | 28 | @Override 29 | public void initialize() { 30 | m_ended = false; 31 | m_command.initialize(); 32 | } 33 | 34 | @Override 35 | public void execute() { 36 | int loops = 0; 37 | while (loops < kMaxLoops) { 38 | if (m_ended) { 39 | m_ended = false; 40 | m_command.initialize(); 41 | } 42 | m_command.execute(); 43 | if (m_command.isFinished()) { 44 | // restart command 45 | m_command.end(false); 46 | m_ended = true; 47 | } else { 48 | return; 49 | } 50 | loops++; 51 | } 52 | } 53 | 54 | @Override 55 | public boolean isFinished() { 56 | return false; 57 | } 58 | 59 | @Override 60 | public void end(boolean interrupted) { 61 | // Make sure we didn't already call end() (which would happen if the command finished in the 62 | // last call to our execute()) 63 | if (!m_ended) { 64 | m_command.end(interrupted); 65 | m_ended = true; 66 | } 67 | } 68 | 69 | @Override 70 | public boolean runsWhenDisabled() { 71 | return m_command.runsWhenDisabled(); 72 | } 73 | 74 | @Override 75 | public InterruptionBehavior getInterruptionBehavior() { 76 | return m_command.getInterruptionBehavior(); 77 | } 78 | 79 | @Override 80 | public void initSendable(SendableBuilder builder) { 81 | super.initSendable(builder); 82 | builder.addStringProperty("command", m_command::getName, null); 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/SimDualJoysticksOI.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.operator_interface; 6 | 7 | import edu.wpi.first.wpilibj2.command.button.CommandJoystick; 8 | import edu.wpi.first.wpilibj2.command.button.Trigger; 9 | 10 | /** Class for controlling the robot with two joysticks. */ 11 | public class SimDualJoysticksOI extends OperatorDashboard { 12 | private final CommandJoystick translateJoystick; 13 | private final CommandJoystick rotateJoystick; 14 | private final Trigger[] translateJoystickButtons; 15 | private final Trigger[] rotateJoystickButtons; 16 | 17 | public SimDualJoysticksOI(int translatePort, int rotatePort) { 18 | translateJoystick = new CommandJoystick(translatePort); 19 | rotateJoystick = new CommandJoystick(rotatePort); 20 | 21 | // buttons use 1-based indexing such that the index matches the button number; leave index 0 set 22 | // to null 23 | this.translateJoystickButtons = new Trigger[13]; 24 | this.rotateJoystickButtons = new Trigger[13]; 25 | 26 | for (int i = 1; i < translateJoystickButtons.length; i++) { 27 | translateJoystickButtons[i] = translateJoystick.button(i); 28 | rotateJoystickButtons[i] = rotateJoystick.button(i); 29 | } 30 | } 31 | 32 | // translation joystick 33 | 34 | @Override 35 | public double getTranslateX() { 36 | return -translateJoystick.getY(); 37 | } 38 | 39 | @Override 40 | public double getTranslateY() { 41 | return -translateJoystick.getX(); 42 | } 43 | 44 | @Override 45 | public Trigger getDriveToPoseButton() { 46 | return translateJoystickButtons[1]; 47 | } 48 | 49 | @Override 50 | public Trigger getFieldRelativeButton() { 51 | return translateJoystickButtons[2]; 52 | } 53 | 54 | @Override 55 | public Trigger getResetPoseToVisionButton() { 56 | return translateJoystickButtons[3]; 57 | } 58 | 59 | // rotation joystick 60 | 61 | @Override 62 | public double getRotate() { 63 | return -rotateJoystick.getX(); 64 | } 65 | 66 | @Override 67 | public Trigger getMoveArmMiddlePositionTrigger() { 68 | return rotateJoystickButtons[1]; 69 | } 70 | 71 | @Override 72 | public Trigger getMoveArmHighPositionTrigger() { 73 | return rotateJoystickButtons[2]; 74 | } 75 | 76 | @Override 77 | public Trigger getInterruptAll() { 78 | return rotateJoystickButtons[4]; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/DualJoysticksOI.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.operator_interface; 6 | 7 | import edu.wpi.first.wpilibj2.command.button.CommandJoystick; 8 | import edu.wpi.first.wpilibj2.command.button.Trigger; 9 | 10 | /** Class for controlling the robot with two joysticks. */ 11 | public class DualJoysticksOI extends OperatorDashboard { 12 | private final CommandJoystick translateJoystick; 13 | private final CommandJoystick rotateJoystick; 14 | private final Trigger[] translateJoystickButtons; 15 | private final Trigger[] rotateJoystickButtons; 16 | 17 | public DualJoysticksOI(int translatePort, int rotatePort) { 18 | translateJoystick = new CommandJoystick(translatePort); 19 | rotateJoystick = new CommandJoystick(rotatePort); 20 | 21 | // buttons use 1-based indexing such that the index matches the button number; leave index 0 set 22 | // to null 23 | this.translateJoystickButtons = new Trigger[13]; 24 | this.rotateJoystickButtons = new Trigger[13]; 25 | 26 | for (int i = 1; i < translateJoystickButtons.length; i++) { 27 | translateJoystickButtons[i] = translateJoystick.button(i); 28 | rotateJoystickButtons[i] = rotateJoystick.button(i); 29 | } 30 | } 31 | 32 | // Translation Joystick 33 | @Override 34 | public double getTranslateX() { 35 | return -translateJoystick.getY(); 36 | } 37 | 38 | @Override 39 | public double getTranslateY() { 40 | return -translateJoystick.getX(); 41 | } 42 | 43 | @Override 44 | public Trigger getDriveToPoseButton() { 45 | return translateJoystickButtons[1]; 46 | } 47 | 48 | @Override 49 | public Trigger getInterruptAll() { 50 | return translateJoystickButtons[5]; 51 | } 52 | 53 | @Override 54 | public Trigger getResetGyroButton() { 55 | return translateJoystickButtons[8]; 56 | } 57 | 58 | @Override 59 | public Trigger getFieldRelativeButton() { 60 | return translateJoystickButtons[9]; 61 | } 62 | 63 | // Rotation Joystick 64 | @Override 65 | public double getRotate() { 66 | return -rotateJoystick.getX(); 67 | } 68 | 69 | @Override 70 | public Trigger getOverrideDriveToPoseButton() { 71 | return rotateJoystickButtons[4]; 72 | } 73 | 74 | @Override 75 | public Trigger getResetPoseToVisionButton() { 76 | return rotateJoystickButtons[5]; 77 | } 78 | 79 | @Override 80 | public Trigger getCurrentPoseButton() { 81 | return rotateJoystickButtons[6]; 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /src/main/deploy/2024-preseason-vision.json: -------------------------------------------------------------------------------- 1 | { 2 | "tags": [ 3 | { 4 | "ID": 1, 5 | "pose": { 6 | "translation": { 7 | "x": 7.1628, 8 | "y": 0, 9 | "z": 0.29 10 | }, 11 | "rotation": { 12 | "quaternion": { 13 | "W": 0.7071067811865475, 14 | "X": 0.0, 15 | "Y": 0.0, 16 | "Z": 0.7071067811865475 17 | } 18 | } 19 | } 20 | }, 21 | { 22 | "ID": 4, 23 | "pose": { 24 | "translation": { 25 | "x": 3.6068, 26 | "y": 0, 27 | "z": 0.254 28 | }, 29 | "rotation": { 30 | "quaternion": { 31 | "W": 0.7071067811865475, 32 | "X": 0.0, 33 | "Y": 0.0, 34 | "Z": 0.7071067811865475 35 | } 36 | } 37 | } 38 | }, 39 | { 40 | "ID": 2, 41 | "pose": { 42 | "translation": { 43 | "x": 0, 44 | "y": 2.286, 45 | "z": 0.508 46 | }, 47 | "rotation": { 48 | "quaternion": { 49 | "W": 1.0, 50 | "X": 0.0, 51 | "Y": 0.0, 52 | "Z": 0.0 53 | } 54 | } 55 | } 56 | }, 57 | { 58 | "ID": 3, 59 | "pose": { 60 | "translation": { 61 | "x": 0, 62 | "y": 2.8448, 63 | "z": 0.508 64 | }, 65 | "rotation": { 66 | "quaternion": { 67 | "W": 1.0, 68 | "X": 0.0, 69 | "Y": 0.0, 70 | "Z": 0.0 71 | } 72 | } 73 | } 74 | }, 75 | { 76 | "ID": 5, 77 | "pose": { 78 | "translation": { 79 | "x": 3.6068, 80 | "y": 3.175, 81 | "z": 1.22555 82 | }, 83 | "rotation": { 84 | "quaternion": { 85 | "W": -0.7071067811865475, 86 | "X": 0.0, 87 | "Y": 0.0, 88 | "Z": 0.7071067811865476 89 | } 90 | } 91 | } 92 | }, 93 | { 94 | "ID": 6, 95 | "pose": { 96 | "translation": { 97 | "x": 7.1628, 98 | "y": 3.175, 99 | "z": 1.3081 100 | }, 101 | "rotation": { 102 | "quaternion": { 103 | "W": -0.7071067811865475, 104 | "X": 0.0, 105 | "Y": 0.0, 106 | "Z": 0.7071067811865476 107 | } 108 | } 109 | } 110 | } 111 | ], 112 | "field": { 113 | "length": 10.7188, 114 | "width": 3.175 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java: -------------------------------------------------------------------------------- 1 | package frc.robot.subsystems.elevator; 2 | 3 | import static edu.wpi.first.units.Units.*; 4 | 5 | import edu.wpi.first.units.measure.Distance; 6 | 7 | public class ElevatorConstants { 8 | 9 | public static final String SUBSYSTEM_NAME = "Elevator"; 10 | 11 | public static final int LEAD_MOTOR_ID = 10; 12 | public static final int FOLLOWER_MOTOR_ID = 11; 13 | 14 | // PID constants are determined empirically through tuning 15 | public static final double KP_SLOT0 = 40.0; 16 | public static final double KI_SLOT0 = 0; 17 | public static final double KD_SLOT0 = 0; 18 | 19 | // feed forward constants are determined through running SysId commands and analyzing the results 20 | // in SysId 21 | public static final double KS_SLOT0 = 0.01; 22 | public static final double KV_SLOT0 = 0.67505; 23 | public static final double KA_SLOT0 = 0.027564; 24 | public static final double KG_SLOT0 = 0.33833; 25 | 26 | // Motion magic constants are determined empirically through tuning 27 | public static final double KV_EXPO = 0.6; 28 | public static final double KA_EXPO = 0.15; 29 | public static final double CRUISE_VELOCITY = 0; // don't limit the cruise velocity 30 | 31 | // the following are determined based on the mechanical design of the elevator 32 | public static final boolean IS_INVERTED = true; 33 | public static final double PULLEY_CIRCUMFERENCE_INCHES = 5.9055; 34 | public static final int GEAR_RATIO = 5; 35 | public static final double ELEVATOR_MASS_KG = 4.5; 36 | public static final Distance MAX_HEIGHT = Inches.of(74); 37 | public static final Distance MIN_HEIGHT = Inches.of(0.0); 38 | public static final double TOLERANCE_INCHES = 0.25; 39 | 40 | public static final double ELEVATOR_RAISE_SLOW_VOLTAGE = 2.0; 41 | public static final double ELEVATOR_LOWERING_SLOW_VOLTAGE = -2.0; 42 | 43 | // This is the current we watch for to detect that the elevator is jammed and needs to be stopped. 44 | public static final double JAMMED_CURRENT = 59.0; 45 | public static final double JAMMED_TIME_THRESHOLD_SECONDS = 0.1; 46 | 47 | // Supply current limits are determined based on current budget for the robot and stator current 48 | // limits are determined based on protecting the mechanism (e.g., don't break the elevator belt). 49 | public static final double ELEVATOR_PEAK_CURRENT_LIMIT = 60.0; 50 | 51 | // example elevator presets of an enumerated value and the associated Distance 52 | public enum Positions { 53 | BOTTOM, 54 | MIDDLE, 55 | TOP 56 | } 57 | 58 | public static final Distance BOTTOM_HEIGHT = Inches.of(0.0); 59 | public static final Distance MIDDLE_HEIGHT = Inches.of(45.0); 60 | public static final Distance TOP_HEIGHT = Inches.of(70.0); 61 | } 62 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/configs/VisionTestPlatformConfig.java: -------------------------------------------------------------------------------- 1 | package frc.robot.configs; 2 | 3 | import static edu.wpi.first.units.Units.*; 4 | 5 | import edu.wpi.first.math.geometry.Rotation3d; 6 | import edu.wpi.first.math.geometry.Transform3d; 7 | import edu.wpi.first.math.geometry.Translation3d; 8 | import edu.wpi.first.units.measure.Angle; 9 | import edu.wpi.first.units.measure.Distance; 10 | import edu.wpi.first.units.measure.LinearVelocity; 11 | import frc.lib.team3061.RobotConfig; 12 | import frc.lib.team3061.swerve_drivetrain.swerve.SwerveConstants; 13 | 14 | /* 15 | * Refer to the README for how to represent your robot's configuration. For more information on 16 | * these methods, refer to the documentation in the RobotConfig class. 17 | */ 18 | public class VisionTestPlatformConfig extends RobotConfig { 19 | private static final String CAMERA_NAME_0 = "photonvisionVTP"; 20 | 21 | // Front right camera 22 | private static final Transform3d ROBOT_TO_CAMERA_0 = 23 | new Transform3d(new Translation3d(), new Rotation3d()); 24 | 25 | @Override 26 | public boolean getPhoenix6Licensed() { 27 | return true; 28 | } 29 | 30 | @Override 31 | public Transform3d[] getRobotToCameraTransforms() { 32 | return new Transform3d[] {ROBOT_TO_CAMERA_0}; 33 | } 34 | 35 | @Override 36 | public String[] getCameraNames() { 37 | return new String[] {CAMERA_NAME_0}; 38 | } 39 | 40 | @Override 41 | public double[] getCameraStdDevFactors() { 42 | return new double[] {1.0}; 43 | } 44 | 45 | @Override 46 | public double getOdometryUpdateFrequency() { 47 | return 250.0; 48 | } 49 | 50 | @Override 51 | public int getPneumaticsHubCANID() { 52 | return 0; 53 | } 54 | 55 | @Override 56 | public LinearVelocity getRobotMaxVelocity() { 57 | return MetersPerSecond.of(0.0); 58 | } 59 | 60 | @Override 61 | public SwerveConstants getSwerveConstants() { 62 | return SwerveConstants.MK4I_L3_PLUS_CONSTANTS; 63 | } 64 | 65 | @Override 66 | public int[] getSwerveDriveMotorCANIDs() { 67 | return new int[] {}; 68 | } 69 | 70 | @Override 71 | public int[] getSwerveSteerMotorCANIDs() { 72 | return new int[] {}; 73 | } 74 | 75 | @Override 76 | public int[] getSwerveSteerEncoderCANIDs() { 77 | return new int[] {}; 78 | } 79 | 80 | @Override 81 | public Angle[] getSwerveSteerOffsets() { 82 | return new Angle[] {}; 83 | } 84 | 85 | @Override 86 | public int getGyroCANID() { 87 | return 0; 88 | } 89 | 90 | @Override 91 | public Distance getTrackwidth() { 92 | return Meters.of(0.0); 93 | } 94 | 95 | @Override 96 | public Distance getWheelbase() { 97 | return Meters.of(0.0); 98 | } 99 | 100 | @Override 101 | public Distance getWheelRadius() { 102 | return Meters.of(0.0); 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/Constants.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.robot; 10 | 11 | import edu.wpi.first.wpilibj.Alert; 12 | import edu.wpi.first.wpilibj.Alert.AlertType; 13 | import edu.wpi.first.wpilibj.RobotBase; 14 | 15 | /** 16 | * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean 17 | * constants. This class should not be used for any other purpose. All constants should be declared 18 | * globally (i.e. public static). Do not put anything functional in this class. 19 | * 20 | *

Subsystem-specific constants should be defined in the subsystem's own constant class. 21 | * Constants that vary from robot to robot should be defined in the config classes. 22 | * 23 | *

It is advised to statically import this class (or one of its inner classes) wherever the 24 | * constants are needed, to reduce verbosity. 25 | */ 26 | public final class Constants { 27 | 28 | // set to true in order to change all Tunable values via AdvantageScope 29 | public static final boolean TUNING_MODE = false; 30 | public static final boolean DEMO_MODE = false; 31 | 32 | private static final RobotType ROBOT = RobotType.ROBOT_COMPETITION; 33 | 34 | private static final Alert invalidRobotAlert = 35 | new Alert("Invalid robot selected, using competition robot as default.", AlertType.kError); 36 | 37 | // FIXME: update for various robots 38 | public enum RobotType { 39 | ROBOT_DEFAULT, 40 | ROBOT_SIMBOT, 41 | ROBOT_XRP, 42 | ROBOT_PRACTICE, 43 | ROBOT_COMPETITION, 44 | ROBOT_PRACTICE_BOARD, 45 | ROBOT_VISION_TEST_PLATFORM 46 | } 47 | 48 | // FIXME: update for various robots 49 | public static RobotType getRobot() { 50 | if (RobotBase.isReal()) { 51 | if (ROBOT == RobotType.ROBOT_SIMBOT 52 | || ROBOT == RobotType.ROBOT_XRP) { // Invalid robot selected 53 | invalidRobotAlert.set(true); 54 | return RobotType.ROBOT_COMPETITION; 55 | } else { 56 | return ROBOT; 57 | } 58 | } else { 59 | return ROBOT; 60 | } 61 | } 62 | 63 | // FIXME: update for various robots 64 | public static Mode getMode() { 65 | switch (getRobot()) { 66 | case ROBOT_DEFAULT, ROBOT_PRACTICE, ROBOT_PRACTICE_BOARD, ROBOT_COMPETITION: 67 | return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; 68 | 69 | case ROBOT_SIMBOT, ROBOT_VISION_TEST_PLATFORM, ROBOT_XRP: 70 | return Mode.SIM; 71 | 72 | default: 73 | return Mode.REAL; 74 | } 75 | } 76 | 77 | public enum Mode { 78 | REAL, 79 | REPLAY, 80 | SIM 81 | } 82 | 83 | public static final double LOOP_PERIOD_SECS = 0.02; 84 | } 85 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/vision/VisionIOSim.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.vision; 2 | 3 | import static frc.lib.team3061.vision.VisionConstants.*; 4 | 5 | import edu.wpi.first.apriltag.AprilTagFieldLayout; 6 | import edu.wpi.first.math.geometry.Pose2d; 7 | import edu.wpi.first.math.geometry.Rotation2d; 8 | import edu.wpi.first.math.geometry.Transform3d; 9 | import java.util.function.Supplier; 10 | import org.photonvision.simulation.PhotonCameraSim; 11 | import org.photonvision.simulation.SimCameraProperties; 12 | import org.photonvision.simulation.VisionSystemSim; 13 | 14 | /** 15 | * PhotonVision-compatible simulated implementation of the VisionIO interface. Only a single 16 | * VisionIOSim object may be instantiated. It uses the PhotonVision SimVisionSystem to simulates the 17 | * AprilTag targets that would be seen by a camera based on the robot's pose, which is determined 18 | * based on its odometry. 19 | * 20 | *

Due to a current bug in PhotonVision, the simulated camera assumes that the 2024 field layout 21 | * is being used. 22 | */ 23 | public class VisionIOSim extends VisionIOPhotonVision { 24 | private static final double DIAGONAL_FOV = 96.0; // FOV in degrees 25 | private static final int IMG_WIDTH = 1600; // image width in px 26 | private static final int IMG_HEIGHT = 1200; // image heigh in px 27 | 28 | private Supplier poseSupplier; 29 | private VisionSystemSim visionSim; 30 | private PhotonCameraSim cameraSim; 31 | 32 | /** 33 | * Creates a new VisionIOSim object. 34 | * 35 | * @param layout the AprilTag field layout 36 | * @param poseSupplier a Pose2d supplier that returns the robot's pose based on its odometry 37 | * @param robotToCamera the transform from the robot's center to the simulated camera 38 | */ 39 | public VisionIOSim( 40 | String cameraName, 41 | AprilTagFieldLayout layout, 42 | Supplier poseSupplier, 43 | Transform3d robotToCamera) { 44 | super(cameraName, layout); 45 | 46 | this.poseSupplier = poseSupplier; 47 | 48 | this.visionSim = new VisionSystemSim(cameraName); 49 | this.visionSim.addAprilTags(layout); 50 | SimCameraProperties cameraProp = new SimCameraProperties(); 51 | cameraProp.setCalibration(IMG_WIDTH, IMG_HEIGHT, Rotation2d.fromDegrees(DIAGONAL_FOV)); 52 | cameraProp.setCalibError(SIM_AVERAGE_ERROR_PIXELS, SIM_ERROR_STD_DEV_PIXELS); 53 | cameraProp.setFPS(15); 54 | cameraProp.setAvgLatencyMs(100); 55 | cameraProp.setLatencyStdDevMs(30); 56 | 57 | this.cameraSim = new PhotonCameraSim(camera, cameraProp, layout); 58 | 59 | visionSim.addCamera(cameraSim, robotToCamera); 60 | } 61 | 62 | /** 63 | * Updates the specified VisionIOInputs object with the latest data from the camera. 64 | * 65 | * @param inputs the VisionIOInputs object to update with the latest data from the camera 66 | */ 67 | @Override 68 | public void updateInputs(VisionIOInputs inputs) { 69 | this.visionSim.update(poseSupplier.get()); 70 | super.updateInputs(inputs); 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/leds/LEDsRIO.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.leds; 2 | 3 | import edu.wpi.first.wpilibj.AddressableLED; 4 | import edu.wpi.first.wpilibj.AddressableLEDBuffer; 5 | import edu.wpi.first.wpilibj.util.Color; 6 | import edu.wpi.first.wpilibj.util.Color8Bit; 7 | import frc.robot.Constants; 8 | 9 | public class LEDsRIO extends LEDs { 10 | 11 | private final AddressableLED leds; 12 | private final AddressableLEDBuffer buffer; 13 | private boolean isGRB; 14 | private boolean competitionBrightness; 15 | 16 | protected LEDsRIO() { 17 | leds = new AddressableLED(0); 18 | 19 | buffer = new AddressableLEDBuffer(ACTUAL_LENGTH); 20 | // leds.setBitTiming(500, 200, 1200, 1300); 21 | isGRB = Constants.getMode() != Constants.Mode.SIM; 22 | competitionBrightness = true; 23 | 24 | leds.setLength(ACTUAL_LENGTH); 25 | leds.setData(buffer); 26 | leds.start(); 27 | } 28 | 29 | @Override 30 | protected void updateLEDs() { 31 | leds.setData(buffer); 32 | } 33 | 34 | @Override 35 | protected void setLEDBuffer(int index, Color color) { 36 | int h = 0; 37 | int s = 0; 38 | int v = 0; 39 | if (isGRB) { 40 | color = changeToGRB(color); 41 | int[] hsv = 42 | RGBToHSV((int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255)); 43 | h = hsv[0]; 44 | s = hsv[1]; 45 | v = hsv[2]; 46 | } 47 | 48 | if (competitionBrightness) { 49 | buffer.setLED(index, color); 50 | } else { 51 | buffer.setHSV(index, h, s, v); 52 | } 53 | 54 | if (MIRROR_LEDS) { 55 | if (competitionBrightness) { 56 | buffer.setLED(ACTUAL_LENGTH - index - 1, color); 57 | } else { 58 | buffer.setHSV(ACTUAL_LENGTH - index - 1, h, s, v); 59 | } 60 | } 61 | } 62 | 63 | @Override 64 | public Color8Bit getColor(int index) { 65 | return new Color8Bit(buffer.getLED(index)); 66 | } 67 | 68 | static Color changeToGRB(Color color) { 69 | return new Color(color.green, color.red, color.blue); 70 | } 71 | 72 | static int[] RGBToHSV(int r, int g, int b) { 73 | double red = r / 255.0; 74 | double green = g / 255.0; 75 | double blue = b / 255.0; 76 | 77 | double cMax = Math.max(red, Math.max(green, blue)); 78 | double cMin = Math.min(red, Math.min(green, blue)); 79 | 80 | double delta = cMax - cMin; 81 | 82 | // Hue 83 | int hue; 84 | 85 | if (delta == 0) { 86 | hue = 0; 87 | } else if (cMax == red) { 88 | hue = (int) Math.round(60 * (((green - blue) / delta) % 6)); 89 | } else if (cMax == green) { 90 | hue = (int) Math.round(60 * (((blue - red) / delta) + 2)); 91 | } else { 92 | hue = (int) Math.round(60 * (((red - green) / delta) + 4)); 93 | } 94 | 95 | // Saturation 96 | double saturation = (cMax == 0) ? 0 : delta / cMax; 97 | 98 | // Convert final values to correct range 99 | 100 | return new int[] { 101 | hue / 2, (int) Math.round(saturation * 255 / 2), (int) Math.round(cMax * 255) 102 | }; 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/swerve_drivetrain/swerve/Conversions.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Initially from https://github.com/Team364/BaseFalconSwerve 3 | */ 4 | 5 | package frc.lib.team3061.swerve_drivetrain.swerve; 6 | 7 | public class Conversions { 8 | 9 | /** 10 | * @param falconRotations Falcon rotations 11 | * @param gearRatio gear ratio between Falcon and mechanism 12 | * @return degrees of rotation of mechanism 13 | */ 14 | public static double falconRotationsToMechanismDegrees(double falconRotations, double gearRatio) { 15 | return falconRotations * 360.0 / gearRatio; 16 | } 17 | 18 | /** 19 | * @param degrees Degrees of rotation of mechanism 20 | * @param gearRatio gear ratio between Falcon and mechanism 21 | * @return Falcon rotations 22 | */ 23 | public static double degreesToFalconRotations(double degrees, double gearRatio) { 24 | return (degrees / 360.0) * gearRatio; 25 | } 26 | 27 | /** 28 | * @param rps Falcon rotations per second 29 | * @param gearRatio gear ratio between Falcon and mechanism (set to 1 for Falcon RPM) 30 | * @return RPM of mechanism 31 | */ 32 | public static double falconRPSToMechanismRPM(double falconRPS, double gearRatio) { 33 | double motorRPM = falconRPS * 60.0; 34 | return motorRPM / gearRatio; 35 | } 36 | 37 | /** 38 | * @param RPM RPM of mechanism 39 | * @param gearRatio Gear ratio between Falcon and mechanism (set to 1 for Falcon RPS) 40 | * @return Falcon rotations per second 41 | */ 42 | public static double rpmToFalconRPS(double rpm, double gearRatio) { 43 | double motorRPM = rpm * gearRatio; 44 | return motorRPM / 60.0; 45 | } 46 | 47 | /** 48 | * @param falconRotations Falcon rotations 49 | * @param circumference circumference of wheel 50 | * @param gearRatio gear ratio between Falcon and mechanism 51 | * @return linear distance traveled by wheel in meters 52 | */ 53 | public static double falconRotationsToMechanismMeters( 54 | double falconRotations, double circumference, double gearRatio) { 55 | double wheelRotations = falconRotations / gearRatio; 56 | return (wheelRotations * circumference); 57 | } 58 | 59 | /** 60 | * @param falconRPS Falcon rotations per second 61 | * @param circumference circumference of wheel 62 | * @param gearRatio gear ratio between Falcon and mechanism 63 | * @return mechanism linear velocity in meters per second 64 | */ 65 | public static double falconRPSToMechanismMPS( 66 | double falconRPS, double circumference, double gearRatio) { 67 | double wheelRPM = falconRPSToMechanismRPM(falconRPS, gearRatio); 68 | return (wheelRPM * circumference) / 60; 69 | } 70 | 71 | /** 72 | * @param velocity velocity in meters per second 73 | * @param circumference circumference of wheel 74 | * @param gearRatio gear ratio between Falcon and mechanism 75 | * @return Falcon rotations per second 76 | */ 77 | public static double mpsToFalconRPS(double velocity, double circumference, double gearRatio) { 78 | double wheelRPM = ((velocity * 60) / circumference); 79 | return rpmToFalconRPS(wheelRPM, gearRatio); 80 | } 81 | 82 | private Conversions() {} 83 | } 84 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "java.configuration.updateBuildConfiguration": "automatic", 3 | "java.server.launchMode": "Standard", 4 | "files.exclude": { 5 | "**/.git": true, 6 | "**/.svn": true, 7 | "**/.hg": true, 8 | "**/CVS": true, 9 | "**/.DS_Store": true, 10 | "bin/": true, 11 | "**/.classpath": true, 12 | "**/.project": true, 13 | "**/.settings": true, 14 | "**/.factorypath": true, 15 | "**/*~": true 16 | }, 17 | "java.test.config": [ 18 | { 19 | "name": "WPIlibUnitTests", 20 | "workingDirectory": "${workspaceFolder}/build/jni/release", 21 | "vmargs": [ 22 | "-Djava.library.path=${workspaceFolder}/build/jni/release" 23 | ], 24 | "env": { 25 | "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", 26 | "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" 27 | } 28 | }, 29 | null 30 | ], 31 | "java.test.defaultConfig": "WPIlibUnitTests", 32 | "spotlessGradle.format.enable": true, 33 | "spotlessGradle.diagnostics.enable": false, 34 | "java.import.gradle.annotationProcessing.enabled": false, 35 | "editor.codeActionsOnSave": { 36 | "source.fixAll.spotlessGradle": "explicit" 37 | }, 38 | "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", 39 | "[json]": { 40 | "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" 41 | }, 42 | "[java]": { 43 | "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" 44 | }, 45 | "cSpell.enabledFileTypes": { 46 | "*": true, 47 | "json": false, 48 | "gradle": false 49 | }, 50 | "cSpell.words": [ 51 | "algs", 52 | "APRILTAG", 53 | "autoset", 54 | "bezier", 55 | "brushless", 56 | "canbus", 57 | "Cancoder", 58 | "CANID", 59 | "CANIDs", 60 | "canivore", 61 | "chargedup", 62 | "Chezy", 63 | "Choreo", 64 | "closedloop", 65 | "CTRE", 66 | "CTRETCF", 67 | "curr", 68 | "Daqs", 69 | "DEADBAND", 70 | "debouncer", 71 | "Devs", 72 | "dtheta", 73 | "feedforward", 74 | "feedforwards", 75 | "gamepad", 76 | "gradlew", 77 | "holonomic", 78 | "Huskie", 79 | "IOCTRE", 80 | "IOXRP", 81 | "jama", 82 | "javac", 83 | "Kalman", 84 | "leds", 85 | "Nivore", 86 | "odometry", 87 | "openloop", 88 | "overriden", 89 | "pathfind", 90 | "pathfinding", 91 | "pathplanner", 92 | "Penrose", 93 | "photonvision", 94 | "PIDF", 95 | "PPLTV", 96 | "pseudoinverse", 97 | "quasistatic", 98 | "rapidreact", 99 | "reproj", 100 | "reprojection", 101 | "REVPHJNI", 102 | "rezero", 103 | "roboRIO", 104 | "scurve", 105 | "Sedgewick", 106 | "selfcheck", 107 | "setpoint", 108 | "setpoints", 109 | "SIMBOT", 110 | "Teleop", 111 | "teleoperated", 112 | "timebase", 113 | "TRACKWIDTH", 114 | "tunables", 115 | "uncomitted", 116 | "untilt", 117 | "untilting", 118 | "Vandermonde", 119 | "VBAT", 120 | "wpilib", 121 | "WPILOG", 122 | "Xstance", 123 | "YUYV" 124 | ] 125 | } 126 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/util/CustomPoseEstimator.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.util; 2 | 3 | import com.ctre.phoenix6.Utils; 4 | import edu.wpi.first.math.Matrix; 5 | import edu.wpi.first.math.geometry.Pose2d; 6 | import edu.wpi.first.math.numbers.N1; 7 | import edu.wpi.first.math.numbers.N3; 8 | import java.util.Optional; 9 | 10 | public interface CustomPoseEstimator { 11 | /** 12 | * Returns the pose as determined by the custom pose estimator of the robot. The origin of the 13 | * field to the lower left corner (i.e., the corner of the field to the driver's right). Zero 14 | * degrees is away from the driver and increases in the CCW direction. 15 | * 16 | * @return the pose as determined by the custom pose estimator of the robot 17 | */ 18 | Pose2d getCustomEstimatedPose(); 19 | 20 | /** 21 | * Return the custom pose at a given timestamp, if the buffer is not empty. 22 | * 23 | * @param timestampSeconds The pose's timestamp. Note that you must use a timestamp with an epoch 24 | * since system startup (i.e., the epoch of this timestamp is the same epoch as {@link 25 | * Utils#getCurrentTimeSeconds}). This means that you should use {@link 26 | * Utils#getCurrentTimeSeconds} as your time source in this case. An FPGA timestamp can be 27 | * converted to the correct timebase using {@link Utils#fpgaToCurrentTime}. 28 | * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). 29 | */ 30 | Optional samplePoseAt(double timestamp); 31 | 32 | /** 33 | * Sets the custom pose estimator of the robot to the specified pose. This method should only be 34 | * invoked when the rotation of the robot is known (e.g., at the start of an autonomous path). The 35 | * origin of the field to the lower left corner (i.e., the corner of the field to the driver's 36 | * right). Zero degrees is away from the driver and increases in the CCW direction. 37 | * 38 | * @param pose the specified pose to which is set the odometry 39 | */ 40 | void resetCustomPose(Pose2d pose); 41 | 42 | /** 43 | * Adds a vision measurement to the Kalman Filter for the custom pose estimator. This will correct 44 | * the odometry pose estimate while still accounting for measurement noise. 45 | * 46 | * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. 47 | * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that you must 48 | * use a timestamp with an epoch since system startup (i.e., the epoch of this timestamp is 49 | * the same epoch as {@link Utils#getCurrentTimeSeconds}). This means that you should use 50 | * {@link Utils#getCurrentTimeSeconds} as your time source or sync the epochs. An FPGA 51 | * timestamp can be converted to the correct timebase using {@link Utils#fpgaToCurrentTime}. 52 | * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position 53 | * in meters, y position in meters, and heading in radians). Increase these numbers to trust 54 | * the vision pose measurement less. 55 | */ 56 | void addVisionMeasurement( 57 | Pose2d visionRobotPoseMeters, 58 | double timestampSeconds, 59 | Matrix visionMeasurementStdDevs); 60 | } 61 | -------------------------------------------------------------------------------- /gradlew.bat: -------------------------------------------------------------------------------- 1 | @rem 2 | @rem Copyright 2015 the original author or authors. 3 | @rem 4 | @rem Licensed under the Apache License, Version 2.0 (the "License"); 5 | @rem you may not use this file except in compliance with the License. 6 | @rem You may obtain a copy of the License at 7 | @rem 8 | @rem https://www.apache.org/licenses/LICENSE-2.0 9 | @rem 10 | @rem Unless required by applicable law or agreed to in writing, software 11 | @rem distributed under the License is distributed on an "AS IS" BASIS, 12 | @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | @rem See the License for the specific language governing permissions and 14 | @rem limitations under the License. 15 | @rem 16 | @rem SPDX-License-Identifier: Apache-2.0 17 | @rem 18 | 19 | @if "%DEBUG%"=="" @echo off 20 | @rem ########################################################################## 21 | @rem 22 | @rem Gradle startup script for Windows 23 | @rem 24 | @rem ########################################################################## 25 | 26 | @rem Set local scope for the variables with windows NT shell 27 | if "%OS%"=="Windows_NT" setlocal 28 | 29 | set DIRNAME=%~dp0 30 | if "%DIRNAME%"=="" set DIRNAME=. 31 | @rem This is normally unused 32 | set APP_BASE_NAME=%~n0 33 | set APP_HOME=%DIRNAME% 34 | 35 | @rem Resolve any "." and ".." in APP_HOME to make it shorter. 36 | for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi 37 | 38 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. 39 | set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" 40 | 41 | @rem Find java.exe 42 | if defined JAVA_HOME goto findJavaFromJavaHome 43 | 44 | set JAVA_EXE=java.exe 45 | %JAVA_EXE% -version >NUL 2>&1 46 | if %ERRORLEVEL% equ 0 goto execute 47 | 48 | echo. 1>&2 49 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 50 | echo. 1>&2 51 | echo Please set the JAVA_HOME variable in your environment to match the 1>&2 52 | echo location of your Java installation. 1>&2 53 | 54 | goto fail 55 | 56 | :findJavaFromJavaHome 57 | set JAVA_HOME=%JAVA_HOME:"=% 58 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe 59 | 60 | if exist "%JAVA_EXE%" goto execute 61 | 62 | echo. 1>&2 63 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 64 | echo. 1>&2 65 | echo Please set the JAVA_HOME variable in your environment to match the 1>&2 66 | echo location of your Java installation. 1>&2 67 | 68 | goto fail 69 | 70 | :execute 71 | @rem Setup the command line 72 | 73 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar 74 | 75 | 76 | @rem Execute Gradle 77 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* 78 | 79 | :end 80 | @rem End local scope for the variables with windows NT shell 81 | if %ERRORLEVEL% equ 0 goto mainEnd 82 | 83 | :fail 84 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of 85 | rem the _cmd.exe /c_ return code! 86 | set EXIT_CODE=%ERRORLEVEL% 87 | if %EXIT_CODE% equ 0 set EXIT_CODE=1 88 | if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% 89 | exit /b %EXIT_CODE% 90 | 91 | :mainEnd 92 | if "%OS%"=="Windows_NT" endlocal 93 | 94 | :omega 95 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/differential_drivetrain/DifferentialDrivetrainIO.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.differential_drivetrain; 2 | 3 | import edu.wpi.first.math.kinematics.ChassisSpeeds; 4 | import edu.wpi.first.units.measure.Force; 5 | import frc.lib.team3061.swerve_drivetrain.SwerveDrivetrainConstants.SysIDCharacterizationMode; 6 | import org.littletonrobotics.junction.AutoLog; 7 | 8 | public interface DifferentialDrivetrainIO { 9 | @AutoLog 10 | public static class DifferentialDrivetrainIOInputs { 11 | double leftMotorSpeed = 0.0; 12 | double rightMotorSpeed = 0.0; 13 | 14 | double leftEncoderCount = 0.0; 15 | double rightEncoderCount = 0.0; 16 | 17 | double leftPositionMeters = 0.0; 18 | double rightPositionMeters = 0.0; 19 | 20 | double leftVelocityMetersPerSecond = 0.0; 21 | double rightVelocityMetersPerSecond = 0.0; 22 | 23 | double headingDeg = 0.0; 24 | double pitchDeg = 0.0; 25 | double rollDeg = 0.0; 26 | 27 | double xAccelerationG = 0.0; 28 | double yAccelerationG = 0.0; 29 | double zAccelerationG = 0.0; 30 | } 31 | 32 | /** Updates the set of loggable inputs. */ 33 | public default void updateInputs(DifferentialDrivetrainIOInputs inputs) {} 34 | 35 | /** 36 | * Controls the drivetrain to move the robot with the desired velocities in the x, y, and 37 | * rotational directions. The velocities are specified from the robot's frame of reference. In the 38 | * robot frame of reference, The origin of the robot is always the center of the robot. The 39 | * positive x direction is forward; the positive y direction, left. Zero degrees is aligned to the 40 | * positive x axis and increases in the CCW direction. 41 | * 42 | * @param xVelocity the desired velocity in the x direction (m/s) 43 | * @param rotationalVelocity the desired rotational velocity (rad/s) 44 | * @param isOpenLoop true for open-loop control; false for closed-loop control 45 | */ 46 | public default void driveRobotRelative( 47 | double xVelocity, double rotationalVelocity, boolean isOpenLoop) {} 48 | 49 | /** 50 | * Controls the drivetrain to move the robot with the desired velocities in the x, y, and 51 | * rotational directions. The velocities are specified from the robot's frame of reference. In the 52 | * robot frame of reference, The origin of the robot is always the center of the robot. The 53 | * positive x direction is forward; the positive y direction, left. Zero degrees is aligned to the 54 | * positive x axis and increases in the CCW direction. 55 | * 56 | * @param speeds the desired chassis speeds 57 | * @param forcesX the robot-centric wheel forces in the x direction 58 | * @param forcesY the robot-centric wheel forces in the y direction 59 | * @param isOpenLoop true for open-loop control; false for closed-loop control 60 | */ 61 | public default void applyRobotSpeeds( 62 | ChassisSpeeds speeds, Force[] forcesX, Force[] forcesY, boolean isOpenLoop) {} 63 | 64 | /** 65 | * Applies the specified characterization mode to the drivetrain with the specified value. This is 66 | * used to characterize the robot for the purpose of system identification. 67 | * 68 | * @param mode the specified characterization mode 69 | * @param value the value to apply to the characterization mode 70 | */ 71 | public default void applySysIdCharacterization(SysIDCharacterizationMode mode, double value) {} 72 | } 73 | -------------------------------------------------------------------------------- /src/main/deploy/2023-chargedup.json: -------------------------------------------------------------------------------- 1 | { 2 | "tags": [ 3 | { 4 | "ID": 1, 5 | "pose": { 6 | "translation": { 7 | "x": 15.513558, 8 | "y": 1.071626, 9 | "z": 0.462788 10 | }, 11 | "rotation": { 12 | "quaternion": { 13 | "W": 0.0, 14 | "X": 0.0, 15 | "Y": 0.0, 16 | "Z": 1.0 17 | } 18 | } 19 | } 20 | }, 21 | { 22 | "ID": 2, 23 | "pose": { 24 | "translation": { 25 | "x": 15.513558, 26 | "y": 2.748026, 27 | "z": 0.462788 28 | }, 29 | "rotation": { 30 | "quaternion": { 31 | "W": 0.0, 32 | "X": 0.0, 33 | "Y": 0.0, 34 | "Z": 1.0 35 | } 36 | } 37 | } 38 | }, 39 | { 40 | "ID": 3, 41 | "pose": { 42 | "translation": { 43 | "x": 15.513558, 44 | "y": 4.424426, 45 | "z": 0.462788 46 | }, 47 | "rotation": { 48 | "quaternion": { 49 | "W": 0.0, 50 | "X": 0.0, 51 | "Y": 0.0, 52 | "Z": 1.0 53 | } 54 | } 55 | } 56 | }, 57 | { 58 | "ID": 4, 59 | "pose": { 60 | "translation": { 61 | "x": 16.178784, 62 | "y": 6.749796, 63 | "z": 0.695452 64 | }, 65 | "rotation": { 66 | "quaternion": { 67 | "W": 0.0, 68 | "X": 0.0, 69 | "Y": 0.0, 70 | "Z": 1.0 71 | } 72 | } 73 | } 74 | }, 75 | { 76 | "ID": 5, 77 | "pose": { 78 | "translation": { 79 | "x": 0.36195, 80 | "y": 6.749796, 81 | "z": 0.695452 82 | }, 83 | "rotation": { 84 | "quaternion": { 85 | "W": 1.0, 86 | "X": 0.0, 87 | "Y": 0.0, 88 | "Z": 0.0 89 | } 90 | } 91 | } 92 | }, 93 | { 94 | "ID": 6, 95 | "pose": { 96 | "translation": { 97 | "x": 1.02743, 98 | "y": 4.424426, 99 | "z": 0.462788 100 | }, 101 | "rotation": { 102 | "quaternion": { 103 | "W": 1.0, 104 | "X": 0.0, 105 | "Y": 0.0, 106 | "Z": 0.0 107 | } 108 | } 109 | } 110 | }, 111 | { 112 | "ID": 7, 113 | "pose": { 114 | "translation": { 115 | "x": 1.02743, 116 | "y": 2.748026, 117 | "z": 0.462788 118 | }, 119 | "rotation": { 120 | "quaternion": { 121 | "W": 1.0, 122 | "X": 0.0, 123 | "Y": 0.0, 124 | "Z": 0.0 125 | } 126 | } 127 | } 128 | }, 129 | { 130 | "ID": 8, 131 | "pose": { 132 | "translation": { 133 | "x": 1.02743, 134 | "y": 1.071626, 135 | "z": 0.462788 136 | }, 137 | "rotation": { 138 | "quaternion": { 139 | "W": 1.0, 140 | "X": 0.0, 141 | "Y": 0.0, 142 | "Z": 0.0 143 | } 144 | } 145 | } 146 | } 147 | ], 148 | "field": { 149 | "length": 16.54175, 150 | "width": 8.0137 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3015/subsystem/selfcheck/SelfCheckingCANCoder.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3015.subsystem.selfcheck; 2 | 3 | import com.ctre.phoenix6.StatusSignal; 4 | import com.ctre.phoenix6.hardware.CANcoder; 5 | import edu.wpi.first.math.filter.Debouncer; 6 | import edu.wpi.first.units.measure.Voltage; 7 | import frc.lib.team3015.subsystem.SubsystemFault; 8 | import java.util.ArrayList; 9 | import java.util.List; 10 | 11 | public class SelfCheckingCANCoder implements SelfChecking { 12 | private final String label; 13 | private final CANcoder canCoder; 14 | private StatusSignal statusSignal; 15 | private final Debouncer connectedDebounce = new Debouncer(0.5); 16 | private final List faults = new ArrayList<>(); 17 | 18 | public SelfCheckingCANCoder(String label, CANcoder canCoder) { 19 | this.label = label; 20 | this.canCoder = canCoder; 21 | this.statusSignal = this.canCoder.getSupplyVoltage().clone(); 22 | } 23 | 24 | @Override 25 | public List checkForFaults() { 26 | faults.clear(); 27 | 28 | // faults 29 | if (canCoder.getFault_Hardware().getValue() == Boolean.TRUE) { 30 | faults.add(new SubsystemFault(String.format("[%s]: Hardware fault detected", label))); 31 | } 32 | if (canCoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { 33 | faults.add(new SubsystemFault(String.format("[%s]: Device booted while enabled", label))); 34 | } 35 | if (canCoder.getFault_BadMagnet().getValue() == Boolean.TRUE) { 36 | faults.add(new SubsystemFault(String.format("[%s]: Bad magnet", label))); 37 | } 38 | if (canCoder.getFault_Undervoltage().getValue() == Boolean.TRUE) { 39 | faults.add( 40 | new SubsystemFault(String.format("[%s]: Device supply voltage near brownout", label))); 41 | } 42 | if (canCoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { 43 | faults.add(new SubsystemFault(String.format("[%s]: Unlicensed feature in use", label))); 44 | } 45 | 46 | // sticky faults 47 | if (canCoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) { 48 | faults.add( 49 | new SubsystemFault(String.format("[%s]: [STICKY] Hardware fault detected", label))); 50 | } 51 | if (canCoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { 52 | faults.add( 53 | new SubsystemFault(String.format("[%s]: [STICKY] Device booted while enabled", label))); 54 | } 55 | if (canCoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) { 56 | faults.add(new SubsystemFault(String.format("[%s]: [STICKY] Bad magnet", label))); 57 | } 58 | if (canCoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { 59 | faults.add( 60 | new SubsystemFault( 61 | String.format("[%s]: [STICKY] Device supply voltage near brownout", label))); 62 | } 63 | if (canCoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { 64 | faults.add( 65 | new SubsystemFault(String.format("[%s]: [STICKY] Unlicensed feature in use", label))); 66 | } 67 | 68 | this.statusSignal.refresh(); 69 | if (!connectedDebounce.calculate(this.statusSignal.getStatus().isOK())) { 70 | faults.add(new SubsystemFault(String.format("[%s]: device is unreachable", label))); 71 | } 72 | 73 | return faults; 74 | } 75 | 76 | @Override 77 | public void clearStickyFaults() { 78 | canCoder.clearStickyFaults(); 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/util/SysIdRoutineChooser.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.util; 2 | 3 | import edu.wpi.first.wpilibj2.command.Command; 4 | import edu.wpi.first.wpilibj2.command.SelectCommand; 5 | import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; 6 | import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; 7 | import java.util.HashMap; 8 | import java.util.Map; 9 | import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; 10 | 11 | /** 12 | * Singleton class to manage SysId routines for dynamic and quasistatic testing. Each subsystem that 13 | * needs to be characterized registers its SysId routines with this class. It uses a 14 | * LoggedDashboardChooser to allow users to select the desired subsystem's SysId routine from the 15 | * dashboard. 16 | */ 17 | public class SysIdRoutineChooser { 18 | 19 | private static final SysIdRoutineChooser instance = new SysIdRoutineChooser(); 20 | 21 | private Map dynamicForwardRoutines = new HashMap<>(); 22 | private Map dynamicReverseRoutines = new HashMap<>(); 23 | private Map quasistaticForwardRoutines = new HashMap<>(); 24 | private Map quasistaticReverseRoutines = new HashMap<>(); 25 | private Command dynamicForwardSelectorCommand = null; 26 | private Command dynamicBackupSelectorCommand = null; 27 | private Command quasistaticForwardSelectorCommand = null; 28 | private Command quasistaticBackwardSelectorCommand = null; 29 | 30 | private final LoggedDashboardChooser sysIdChooser = 31 | new LoggedDashboardChooser<>("SysId Chooser"); 32 | 33 | private SysIdRoutineChooser() {} 34 | 35 | public static SysIdRoutineChooser getInstance() { 36 | return instance; 37 | } 38 | 39 | public void addOption(String name, SysIdRoutine sysIdRoutine) { 40 | sysIdChooser.addOption(name, name.hashCode()); 41 | dynamicForwardRoutines.put(name.hashCode(), sysIdRoutine.dynamic(Direction.kForward)); 42 | dynamicReverseRoutines.put(name.hashCode(), sysIdRoutine.dynamic(Direction.kReverse)); 43 | quasistaticForwardRoutines.put(name.hashCode(), sysIdRoutine.quasistatic(Direction.kForward)); 44 | quasistaticReverseRoutines.put(name.hashCode(), sysIdRoutine.quasistatic(Direction.kReverse)); 45 | } 46 | 47 | public Command getDynamicForward() { 48 | if (dynamicForwardSelectorCommand == null) { 49 | dynamicForwardSelectorCommand = 50 | new SelectCommand<>(this.dynamicForwardRoutines, sysIdChooser::get); 51 | } 52 | return dynamicForwardSelectorCommand; 53 | } 54 | 55 | public Command getDynamicReverse() { 56 | if (dynamicBackupSelectorCommand == null) { 57 | dynamicBackupSelectorCommand = 58 | new SelectCommand<>(this.dynamicReverseRoutines, sysIdChooser::get); 59 | } 60 | 61 | return dynamicBackupSelectorCommand; 62 | } 63 | 64 | public Command getQuasistaticForward() { 65 | if (quasistaticForwardSelectorCommand == null) { 66 | quasistaticForwardSelectorCommand = 67 | new SelectCommand<>(this.quasistaticForwardRoutines, sysIdChooser::get); 68 | } 69 | 70 | return quasistaticForwardSelectorCommand; 71 | } 72 | 73 | public Command getQuasistaticReverse() { 74 | if (quasistaticBackwardSelectorCommand == null) { 75 | quasistaticBackwardSelectorCommand = 76 | new SelectCommand<>(this.quasistaticReverseRoutines, sysIdChooser::get); 77 | } 78 | return quasistaticBackwardSelectorCommand; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/OperatorInterface.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.robot.operator_interface; 10 | 11 | import edu.wpi.first.wpilibj2.command.button.Trigger; 12 | 13 | /** Interface for all driver and operator controls. */ 14 | public interface OperatorInterface { 15 | 16 | // drivetrain, generic 17 | 18 | public default double getTranslateX() { 19 | return 0.0; 20 | } 21 | 22 | public default double getTranslateY() { 23 | return 0.0; 24 | } 25 | 26 | public default double getRotate() { 27 | return 0.0; 28 | } 29 | 30 | public default Trigger getFieldRelativeButton() { 31 | return new Trigger(() -> false); 32 | } 33 | 34 | public default Trigger getResetGyroButton() { 35 | return new Trigger(() -> false); 36 | } 37 | 38 | public default Trigger getResetPoseToVisionButton() { 39 | return new Trigger(() -> false); 40 | } 41 | 42 | public default Trigger getXStanceButton() { 43 | return new Trigger(() -> false); 44 | } 45 | 46 | public default Trigger getTranslationSlowModeButton() { 47 | return new Trigger(() -> false); 48 | } 49 | 50 | public default Trigger getRotationSlowModeButton() { 51 | return new Trigger(() -> false); 52 | } 53 | 54 | public default Trigger getLock180Button() { 55 | return new Trigger(() -> false); 56 | } 57 | 58 | public default Trigger getVisionIsEnabledTrigger() { 59 | return new Trigger(() -> false); 60 | } 61 | 62 | public default Trigger getSysIdDynamicForward() { 63 | return new Trigger(() -> false); 64 | } 65 | 66 | public default Trigger getSysIdDynamicReverse() { 67 | return new Trigger(() -> false); 68 | } 69 | 70 | public default Trigger getSysIdQuasistaticForward() { 71 | return new Trigger(() -> false); 72 | } 73 | 74 | public default Trigger getSysIdQuasistaticReverse() { 75 | return new Trigger(() -> false); 76 | } 77 | 78 | // DRIVER TRIGGERS, mostly game-specific 79 | 80 | public default Trigger getDriveToPoseButton() { 81 | return new Trigger(() -> false); 82 | } 83 | 84 | public default Trigger getOverrideDriveToPoseButton() { 85 | return new Trigger(() -> false); 86 | } 87 | 88 | public default Trigger getCurrentPoseButton() { 89 | return new Trigger(() -> false); 90 | } 91 | 92 | public default Trigger getInterruptAll() { 93 | return new Trigger(() -> false); 94 | } 95 | 96 | // OPERATOR TRIGGERS, mostly game-specific 97 | public default Trigger getEnablePrimaryIRSensorsTrigger() { 98 | return new Trigger(() -> false); 99 | } 100 | 101 | public default Trigger getEnableAutoScoringTrigger() { 102 | return new Trigger(() -> false); 103 | } 104 | 105 | // XRP EXAMPLE TRIGGERS 106 | public default Trigger getMoveArmMiddlePositionTrigger() { 107 | return new Trigger(() -> false); 108 | } 109 | 110 | public default Trigger getMoveArmHighPositionTrigger() { 111 | return new Trigger(() -> false); 112 | } 113 | 114 | // ELEVATOR EXAMPLE TRIGGERS 115 | public default Trigger getRaiseElevatorSlowButton() { 116 | return new Trigger(() -> false); 117 | } 118 | 119 | public default Trigger getLowerElevatorSlowButton() { 120 | return new Trigger(() -> false); 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # This gitignore has been specially created by the WPILib team. 2 | # If you remove items from this file, intellisense might break. 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | ### Java ### 39 | # Compiled class file 40 | *.class 41 | 42 | # Log file 43 | *.log 44 | 45 | # BlueJ files 46 | *.ctxt 47 | 48 | # Mobile Tools for Java (J2ME) 49 | .mtj.tmp/ 50 | 51 | # Package Files # 52 | *.jar 53 | *.war 54 | *.nar 55 | *.ear 56 | *.zip 57 | *.tar.gz 58 | *.rar 59 | 60 | # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml 61 | hs_err_pid* 62 | 63 | ### Linux ### 64 | *~ 65 | 66 | # temporary files which can be created if a process still has a handle open of a deleted file 67 | .fuse_hidden* 68 | 69 | # KDE directory preferences 70 | .directory 71 | 72 | # Linux trash folder which might appear on any partition or disk 73 | .Trash-* 74 | 75 | # .nfs files are created when an open file is removed but is still being accessed 76 | .nfs* 77 | 78 | ### macOS ### 79 | # General 80 | .DS_Store 81 | .AppleDouble 82 | .LSOverride 83 | 84 | # Icon must end with two \r 85 | Icon 86 | 87 | # Thumbnails 88 | ._* 89 | 90 | # Files that might appear in the root of a volume 91 | .DocumentRevisions-V100 92 | .fseventsd 93 | .Spotlight-V100 94 | .TemporaryItems 95 | .Trashes 96 | .VolumeIcon.icns 97 | .com.apple.timemachine.donotpresent 98 | 99 | # Directories potentially created on remote AFP share 100 | .AppleDB 101 | .AppleDesktop 102 | Network Trash Folder 103 | Temporary Items 104 | .apdisk 105 | 106 | ### VisualStudioCode ### 107 | .vscode/* 108 | !.vscode/settings.json 109 | !.vscode/tasks.json 110 | !.vscode/launch.json 111 | !.vscode/extensions.json 112 | 113 | ### Windows ### 114 | # Windows thumbnail cache files 115 | Thumbs.db 116 | ehthumbs.db 117 | ehthumbs_vista.db 118 | 119 | # Dump file 120 | *.stackdump 121 | 122 | # Folder config file 123 | [Dd]esktop.ini 124 | 125 | # Recycle Bin used on file shares 126 | $RECYCLE.BIN/ 127 | 128 | # Windows Installer files 129 | *.cab 130 | *.msi 131 | *.msix 132 | *.msm 133 | *.msp 134 | 135 | # Windows shortcuts 136 | *.lnk 137 | 138 | ### Gradle ### 139 | .gradle 140 | /build/ 141 | 142 | # Ignore Gradle GUI config 143 | gradle-app.setting 144 | 145 | # Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) 146 | !gradle-wrapper.jar 147 | 148 | # Cache of project 149 | .gradletasknamecache 150 | 151 | # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 152 | # gradle/wrapper/gradle-wrapper.properties 153 | 154 | # # VS Code Specific Java Settings 155 | # DO NOT REMOVE .classpath and .project 156 | .classpath 157 | .project 158 | .settings/ 159 | bin/ 160 | 161 | # IntelliJ 162 | *.iml 163 | *.ipr 164 | *.iws 165 | .idea/ 166 | out/ 167 | 168 | # Fleet 169 | .fleet 170 | 171 | # Simulation GUI and other tools window save file 172 | networktables.json 173 | # simgui.json 174 | *-window.json 175 | 176 | # Simulation data log directory 177 | logs/ 178 | 179 | # Folder that has CTRE Phoenix Sim device config storage 180 | ctre_sim/ 181 | 182 | # clangd 183 | /.cache 184 | compile_commands.json 185 | 186 | # Version file 187 | src/main/java/frc/robot/BuildConstants.java -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/vision/VisionConstants.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.vision; 2 | 3 | import edu.wpi.first.wpilibj.Filesystem; 4 | import java.io.File; 5 | import java.nio.file.Path; 6 | 7 | public final class VisionConstants { 8 | private static final String CONSTRUCTOR_EXCEPTION = "constant class"; 9 | 10 | private VisionConstants() { 11 | throw new IllegalStateException(CONSTRUCTOR_EXCEPTION); 12 | } 13 | 14 | public static final Path APRILTAG_FIELD_LAYOUT_PATH = 15 | new File(Filesystem.getDeployDirectory(), "2025-reefscape.json").toPath(); 16 | public static final Path OFFICIAL_APRILTAG_FIELD_LAYOUT_PATH = 17 | new File(Filesystem.getDeployDirectory(), "2025-reefscape.json").toPath(); 18 | public static final int MAX_NUMBER_TAGS = 30; 19 | 20 | public static final String SUBSYSTEM_NAME = "Vision"; 21 | 22 | public static final boolean ENABLE_DETAILED_LOGGING = false; 23 | public static final boolean CALIBRATE_CAMERA_TRANSFORMS = false; 24 | 25 | // an accepted pose must be within this many seconds of the current time in order to qualify for 26 | // resetting the robot's pose with the reset pose to vision command 27 | public static final double BEST_POSE_TIME_THRESHOLD_SECS = 0.5; 28 | 29 | // AprilTags that were seen more than this many seconds ago are considered stale and are not 30 | // logged 31 | public static final double TAG_LOG_TIME_SECS = 0.1; 32 | 33 | // Vision poses that were estimated more than this many seconds ago are considered stale and are 34 | // not logged 35 | public static final double POSE_LOG_TIME_SECS = 0.1; 36 | 37 | // the pose ambiguity must be less than this value for the target to be considered valid 38 | public static final double AMBIGUITY_THRESHOLD = 0.5; 39 | 40 | // the maximum difference, in degrees, between the robot's current rotation and the rotation 41 | // calculated from the vision target for the pose to be considered valid 42 | public static final double ROTATION_THRESHOLD_DEGREES = 10.0; 43 | 44 | // the reprojection error must be less than this value for the target to be considered valid 45 | public static final double REPROJECTION_ERROR_THRESHOLD = 5.0; 46 | 47 | // the maximum error in the z component of the robot's pose for the pose to be considered valid 48 | // (assumes that the robot is always on the carpet) 49 | public static final double MAX_Z_ERROR_METERS = 0.25; 50 | 51 | // the maximum distance off the field for the robot's pose for the pose to be considered valid 52 | public static final double FIELD_BORDER_MARGIN_METERS = 0.5; 53 | 54 | // this factor is applied to the pose ambiguity when calculating the standard deviation to pass to 55 | // the pose estimator (only used with single-tag estimation) 56 | public static final double AMBIGUITY_SCALE_FACTOR = 5.0; 57 | 58 | // this factor is applied to the pose reprojection error when calculating the standard deviation 59 | // to pass to the pose estimator (only used with multi-tag estimation) 60 | public static final double REPROJECTION_ERROR_SCALE_FACTOR = 3.33; 61 | 62 | // the coefficient from which the standard deviation for the x and y components is initiated 63 | public static final double X_Y_STD_DEV_COEFFICIENT = 0.08; 64 | 65 | // the coefficient from which the standard deviation for the theta component is initiated (only 66 | // used with multi-tag estimation) 67 | public static final double THETA_STD_DEV_COEFFICIENT = 0.1; 68 | 69 | // the average error in pixels for the simulated camera 70 | public static final double SIM_AVERAGE_ERROR_PIXELS = 0.1; 71 | 72 | // te standard deviation of the error in pixels for the simulated camera 73 | public static final double SIM_ERROR_STD_DEV_PIXELS = 0.05; 74 | } 75 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/vision/VisionIOPhotonVision.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.vision; 2 | 3 | import edu.wpi.first.apriltag.AprilTagFieldLayout; 4 | import edu.wpi.first.math.geometry.Transform3d; 5 | import edu.wpi.first.wpilibj.Timer; 6 | import java.util.ArrayList; 7 | import java.util.List; 8 | import java.util.Optional; 9 | import org.photonvision.EstimatedRobotPose; 10 | import org.photonvision.PhotonCamera; 11 | import org.photonvision.PhotonPoseEstimator; 12 | import org.photonvision.PhotonPoseEstimator.PoseStrategy; 13 | import org.photonvision.targeting.PhotonPipelineResult; 14 | 15 | public class VisionIOPhotonVision implements VisionIO { 16 | protected final PhotonCamera camera; 17 | protected PhotonPoseEstimator photonEstimator; 18 | private final List observations = new ArrayList<>(); 19 | 20 | /** 21 | * Creates a new VisionIOPhotonVision object. 22 | * 23 | * @param cameraName the name of the PhotonVision camera to use; the name must be unique 24 | */ 25 | public VisionIOPhotonVision(String cameraName, AprilTagFieldLayout layout) { 26 | this.camera = new PhotonCamera(cameraName); 27 | 28 | // Don't pass the robot to camera transform as we will work with the estimated camera poses and 29 | // later transform them to the robot's frame 30 | this.photonEstimator = 31 | new PhotonPoseEstimator( 32 | layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, new Transform3d()); 33 | 34 | // flush any old results from previous results 35 | this.camera.getAllUnreadResults(); 36 | } 37 | 38 | /** 39 | * Updates the specified VisionIOInputs object with the latest data from the camera. 40 | * 41 | * @param inputs the VisionIOInputs object to update with the latest data from the camera 42 | */ 43 | @Override 44 | public void updateInputs(VisionIOInputs inputs) { 45 | inputs.connected = camera.isConnected(); 46 | observations.clear(); 47 | 48 | for (PhotonPipelineResult result : camera.getAllUnreadResults()) { 49 | Optional visionEstimate = this.photonEstimator.update(result); 50 | 51 | visionEstimate.ifPresent( 52 | estimate -> { 53 | long tagsSeenBitMap = 0; 54 | double averageAmbiguity = 0.0; 55 | double averageTagDistance = 0.0; 56 | 57 | for (int i = 0; i < estimate.targetsUsed.size(); i++) { 58 | tagsSeenBitMap |= 1L << estimate.targetsUsed.get(i).getFiducialId(); 59 | averageAmbiguity += estimate.targetsUsed.get(i).getPoseAmbiguity(); 60 | averageTagDistance += 61 | estimate.targetsUsed.get(i).getBestCameraToTarget().getTranslation().getNorm(); 62 | } 63 | averageAmbiguity /= estimate.targetsUsed.size(); 64 | averageTagDistance /= estimate.targetsUsed.size(); 65 | 66 | observations.add( 67 | new PoseObservation( 68 | result.getTimestampSeconds(), 69 | estimate.estimatedPose, 70 | Timer.getFPGATimestamp() - result.getTimestampSeconds(), 71 | averageAmbiguity, 72 | result.multitagResult.isPresent() 73 | ? result.multitagResult.get().estimatedPose.bestReprojErr 74 | : 0.0, 75 | tagsSeenBitMap, 76 | estimate.targetsUsed.size(), 77 | averageTagDistance, 78 | estimate.strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR 79 | ? PoseObservationType.MULTI_TAG 80 | : PoseObservationType.SINGLE_TAG)); 81 | }); 82 | } 83 | 84 | inputs.poseObservations = observations.toArray(new PoseObservation[0]); 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/util/DifferentialRobotOdometry.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.util; 2 | 3 | import edu.wpi.first.math.Matrix; 4 | import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; 5 | import edu.wpi.first.math.geometry.Pose2d; 6 | import edu.wpi.first.math.geometry.Rotation2d; 7 | import edu.wpi.first.math.numbers.N1; 8 | import edu.wpi.first.math.numbers.N3; 9 | import frc.lib.team3061.RobotConfig; 10 | import java.util.Optional; 11 | import org.littletonrobotics.junction.Logger; 12 | 13 | /** 14 | * Singleton class for SwerveDrivePoseEstimator that allows it to be shared by subsystems 15 | * (drivetrain and vision). This class supports two pose estimators: the main one and a custom one. 16 | * The assumption is that the custom one is below the AdvantageKit hardware abstraction layer and, 17 | * therefore doesn't support tuning and debugging via replay. The pose estimator defined in this 18 | * class is above the AdvantageKit hardware abstraction layer and supports tuning and debugging via 19 | * replay. 20 | */ 21 | public class DifferentialRobotOdometry extends RobotOdometry { 22 | private DifferentialDrivePoseEstimator estimator = null; 23 | 24 | public DifferentialRobotOdometry() { 25 | estimator = 26 | new DifferentialDrivePoseEstimator( 27 | RobotConfig.getInstance().getDifferentialDriveKinematics(), 28 | new Rotation2d(), 29 | 0.0, 30 | 0.0, 31 | new Pose2d()); 32 | } 33 | 34 | /** 35 | * Returns the estimated pose of the robot as determined by this class's pose estimator. 36 | * 37 | * @return the estimated pose of the robot 38 | */ 39 | @Override 40 | public Pose2d getEstimatedPose() { 41 | return this.estimator.getEstimatedPosition(); 42 | } 43 | 44 | /** 45 | * Resets the pose of the robot to the specified pose. This method resets both the main pose 46 | * estimator and the custom pose estimator. 47 | * 48 | * @param gyroAngle the current raw heading of the gyro 49 | * @param modulePositions the current positions of the swerve modules 50 | * @param poseMeters the new pose of the robot 51 | */ 52 | public void resetPose( 53 | Rotation2d gyroAngle, 54 | double leftPositionMeters, 55 | double rightPositionMeters, 56 | Pose2d poseMeters) { 57 | super.resetCustomPose(poseMeters); 58 | this.estimator.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters); 59 | } 60 | 61 | /** 62 | * Updates the pose estimator with the current time, gyro angle, and module positions. The custom 63 | * pose estimator will be updated via its own mechanism. 64 | * 65 | * @param currentTimeSeconds the current time in seconds. Note that you must use a timestamp 66 | * aligned with the FPGA timebase. Note that this is different than the timebase used by CTRE 67 | * which uses an epoch since system startup (i.e., the epoch of this timestamp is the same 68 | * epoch as Utils.getCurrentTimeSeconds()). 69 | * @param gyroAngle the current raw heading of the gyro 70 | * @param modulePositions the current positions of the swerve modules 71 | * @return the estimated pose of the robot 72 | */ 73 | public Pose2d updateWithTime( 74 | double currentTimeSeconds, 75 | Rotation2d gyroAngle, 76 | double distanceLeftMeters, 77 | double distanceRightMeters) { 78 | Logger.recordOutput("RobotOdometry/updateTime", currentTimeSeconds); 79 | return this.estimator.updateWithTime( 80 | currentTimeSeconds, gyroAngle, distanceLeftMeters, distanceRightMeters); 81 | } 82 | 83 | protected void addVisionMeasurement( 84 | Pose2d visionRobotPoseMeters, 85 | double timestampSeconds, 86 | Matrix visionMeasurementStdDevs) { 87 | 88 | this.estimator.addVisionMeasurement( 89 | visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); 90 | } 91 | 92 | protected Optional sampleAt(double timestampSeconds) { 93 | return this.estimator.sampleAt(timestampSeconds); 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team254/ChezySequenceCommandGroup.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team254; 2 | 3 | import edu.wpi.first.util.sendable.SendableBuilder; 4 | import edu.wpi.first.wpilibj2.command.Command; 5 | import edu.wpi.first.wpilibj2.command.CommandScheduler; 6 | import java.util.ArrayList; 7 | import java.util.List; 8 | 9 | /** 10 | * A command composition that runs a list of commands in sequence. But faster. 11 | * 12 | *

The rules for command compositions apply: command instances that are passed to it cannot be 13 | * added to any other composition or scheduled individually, and the composition requires all 14 | * subsystems its components require. 15 | */ 16 | public class ChezySequenceCommandGroup extends Command { 17 | private final List m_commands = new ArrayList<>(); 18 | private int m_currentCommandIndex = -1; 19 | private boolean m_runWhenDisabled = true; 20 | private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming; 21 | 22 | /** 23 | * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the 24 | * composition finishing when the last command finishes. 25 | * 26 | * @param commands the commands to include in this composition. 27 | */ 28 | @SuppressWarnings("this-escape") 29 | public ChezySequenceCommandGroup(Command... commands) { 30 | addCommands(commands); 31 | } 32 | 33 | /** 34 | * Adds the given commands to the group. 35 | * 36 | * @param commands Commands to add, in order of execution. 37 | */ 38 | @SuppressWarnings("PMD.UseArraysAsList") 39 | public final void addCommands(Command... commands) { 40 | if (m_currentCommandIndex != -1) { 41 | throw new IllegalStateException( 42 | "Commands cannot be added to a composition while it's running"); 43 | } 44 | 45 | CommandScheduler.getInstance().registerComposedCommands(commands); 46 | 47 | for (Command command : commands) { 48 | m_commands.add(command); 49 | addRequirements(command.getRequirements()); 50 | m_runWhenDisabled &= command.runsWhenDisabled(); 51 | if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { 52 | m_interruptBehavior = InterruptionBehavior.kCancelSelf; 53 | } 54 | } 55 | } 56 | 57 | @Override 58 | public final void initialize() { 59 | m_currentCommandIndex = 0; 60 | 61 | if (!m_commands.isEmpty()) { 62 | m_commands.get(0).initialize(); 63 | } 64 | } 65 | 66 | @Override 67 | public final void execute() { 68 | if (m_commands.isEmpty()) { 69 | return; 70 | } 71 | 72 | Command currentCommand = m_commands.get(m_currentCommandIndex); 73 | 74 | currentCommand.execute(); 75 | if (currentCommand.isFinished()) { 76 | currentCommand.end(false); 77 | m_currentCommandIndex++; 78 | if (m_currentCommandIndex < m_commands.size()) { 79 | m_commands.get(m_currentCommandIndex).initialize(); 80 | 81 | // Go again to run the next loop. 82 | this.execute(); 83 | } 84 | } 85 | } 86 | 87 | @Override 88 | public final void end(boolean interrupted) { 89 | if (interrupted 90 | && !m_commands.isEmpty() 91 | && m_currentCommandIndex > -1 92 | && m_currentCommandIndex < m_commands.size()) { 93 | m_commands.get(m_currentCommandIndex).end(true); 94 | } 95 | m_currentCommandIndex = -1; 96 | } 97 | 98 | @Override 99 | public final boolean isFinished() { 100 | return m_currentCommandIndex == m_commands.size(); 101 | } 102 | 103 | @Override 104 | public boolean runsWhenDisabled() { 105 | return m_runWhenDisabled; 106 | } 107 | 108 | @Override 109 | public InterruptionBehavior getInterruptionBehavior() { 110 | return m_interruptBehavior; 111 | } 112 | 113 | @Override 114 | public void initSendable(SendableBuilder builder) { 115 | super.initSendable(builder); 116 | 117 | builder.addIntegerProperty("index", () -> m_currentCommandIndex, null); 118 | } 119 | } 120 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/util/SwerveRobotOdometry.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.util; 2 | 3 | import edu.wpi.first.math.Matrix; 4 | import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 5 | import edu.wpi.first.math.geometry.Pose2d; 6 | import edu.wpi.first.math.geometry.Rotation2d; 7 | import edu.wpi.first.math.kinematics.SwerveModulePosition; 8 | import edu.wpi.first.math.numbers.N1; 9 | import edu.wpi.first.math.numbers.N3; 10 | import frc.lib.team3061.RobotConfig; 11 | import java.util.Optional; 12 | import org.littletonrobotics.junction.Logger; 13 | 14 | /** 15 | * Singleton class for SwerveDrivePoseEstimator that allows it to be shared by subsystems 16 | * (drivetrain and vision). This class supports two pose estimators: the main one and a custom one. 17 | * The assumption is that the custom one is below the AdvantageKit hardware abstraction layer and, 18 | * therefore doesn't support tuning and debugging via replay. The pose estimator defined in this 19 | * class is above the AdvantageKit hardware abstraction layer and supports tuning and debugging via 20 | * replay. 21 | */ 22 | public class SwerveRobotOdometry extends RobotOdometry { 23 | private SwerveDrivePoseEstimator estimator = null; 24 | 25 | public SwerveRobotOdometry() { 26 | estimator = 27 | new SwerveDrivePoseEstimator( 28 | RobotConfig.getInstance().getSwerveDriveKinematics(), 29 | new Rotation2d(), 30 | new SwerveModulePosition[] { 31 | new SwerveModulePosition(), 32 | new SwerveModulePosition(), 33 | new SwerveModulePosition(), 34 | new SwerveModulePosition() 35 | }, 36 | new Pose2d()); 37 | } 38 | 39 | /** 40 | * Returns the estimated pose of the robot as determined by this class's pose estimator. 41 | * 42 | * @return the estimated pose of the robot 43 | */ 44 | @Override 45 | public Pose2d getEstimatedPose() { 46 | return this.estimator.getEstimatedPosition(); 47 | } 48 | 49 | /** 50 | * Resets the pose of the robot to the specified pose. This method resets both the main pose 51 | * estimator and the custom pose estimator. 52 | * 53 | * @param gyroAngle the current raw heading of the gyro 54 | * @param modulePositions the current positions of the swerve modules 55 | * @param poseMeters the new pose of the robot 56 | */ 57 | public void resetPose( 58 | Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { 59 | super.resetCustomPose(poseMeters); 60 | this.estimator.resetPosition(gyroAngle, modulePositions, poseMeters); 61 | } 62 | 63 | /** 64 | * Updates the pose estimator with the current time, gyro angle, and module positions. The custom 65 | * pose estimator will be updated via its own mechanism. 66 | * 67 | * @param currentTimeSeconds the current time in seconds. Note that you must use a timestamp 68 | * aligned with the FPGA timebase. Note that this is different than the timebase used by CTRE 69 | * which uses an epoch since system startup (i.e., the epoch of this timestamp is the same 70 | * epoch as Utils.getCurrentTimeSeconds()). 71 | * @param gyroAngle the current raw heading of the gyro 72 | * @param modulePositions the current positions of the swerve modules 73 | * @return the estimated pose of the robot 74 | */ 75 | public Pose2d updateWithTime( 76 | double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { 77 | Logger.recordOutput("RobotOdometry/updateTime", currentTimeSeconds); 78 | return this.estimator.updateWithTime(currentTimeSeconds, gyroAngle, modulePositions); 79 | } 80 | 81 | protected void addVisionMeasurement( 82 | Pose2d visionRobotPoseMeters, 83 | double timestampSeconds, 84 | Matrix visionMeasurementStdDevs) { 85 | 86 | this.estimator.addVisionMeasurement( 87 | visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); 88 | } 89 | 90 | protected Optional sampleAt(double timestampSeconds) { 91 | return this.estimator.sampleAt(timestampSeconds); 92 | } 93 | } 94 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/FullOperatorConsoleOI.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | package frc.robot.operator_interface; 6 | 7 | import edu.wpi.first.wpilibj2.command.button.CommandJoystick; 8 | import edu.wpi.first.wpilibj2.command.button.CommandXboxController; 9 | import edu.wpi.first.wpilibj2.command.button.Trigger; 10 | 11 | /** 12 | * Class for controlling the robot with two joysticks, 1 Xbox controller, and 1 operator button 13 | * panel. 14 | */ 15 | public class FullOperatorConsoleOI extends OperatorDashboard { 16 | private final CommandJoystick translateJoystick; 17 | private final Trigger[] translateJoystickButtons; 18 | 19 | private final CommandJoystick rotateJoystick; 20 | private final Trigger[] rotateJoystickButtons; 21 | 22 | private final CommandXboxController operatorController; 23 | 24 | private final CommandJoystick operatorPanel; 25 | private final Trigger[] operatorPanelButtons; 26 | 27 | public FullOperatorConsoleOI( 28 | int translatePort, int rotatePort, int operatorControllerPort, int operatorPanelPort) { 29 | translateJoystick = new CommandJoystick(translatePort); 30 | rotateJoystick = new CommandJoystick(rotatePort); 31 | operatorController = new CommandXboxController(operatorControllerPort); 32 | operatorPanel = new CommandJoystick(operatorPanelPort); 33 | 34 | // buttons use 1-based indexing such that the index matches the button number; leave index 0 set 35 | // to null 36 | this.translateJoystickButtons = new Trigger[13]; 37 | this.rotateJoystickButtons = new Trigger[13]; 38 | this.operatorPanelButtons = new Trigger[13]; 39 | 40 | for (int i = 1; i < translateJoystickButtons.length; i++) { 41 | translateJoystickButtons[i] = translateJoystick.button(i); 42 | rotateJoystickButtons[i] = rotateJoystick.button(i); 43 | } 44 | for (int i = 1; i < operatorPanelButtons.length; i++) { 45 | operatorPanelButtons[i] = operatorPanel.button(i); 46 | } 47 | } 48 | 49 | // Translate Joystick 50 | @Override 51 | public double getTranslateX() { 52 | return -translateJoystick.getY(); 53 | } 54 | 55 | @Override 56 | public double getTranslateY() { 57 | return -translateJoystick.getX(); 58 | } 59 | 60 | @Override 61 | public Trigger getLock180Button() { 62 | return translateJoystickButtons[2]; 63 | } 64 | 65 | @Override 66 | public Trigger getResetGyroButton() { 67 | return translateJoystickButtons[4]; 68 | } 69 | 70 | @Override 71 | public Trigger getFieldRelativeButton() { 72 | return translateJoystickButtons[9]; 73 | } 74 | 75 | // Rotate Joystick 76 | 77 | @Override 78 | public double getRotate() { 79 | return -rotateJoystick.getX(); 80 | } 81 | 82 | @Override 83 | public Trigger getXStanceButton() { 84 | return rotateJoystickButtons[4]; 85 | } 86 | 87 | @Override 88 | public Trigger getResetPoseToVisionButton() { 89 | return rotateJoystickButtons[5]; 90 | } 91 | 92 | // Operator Controller 93 | @Override 94 | public Trigger getInterruptAll() { 95 | return operatorController.start(); 96 | } 97 | 98 | @Override 99 | public Trigger getSysIdDynamicForward() { 100 | return operatorController.back().and(operatorController.y()); 101 | } 102 | 103 | @Override 104 | public Trigger getSysIdDynamicReverse() { 105 | return operatorController.back().and(operatorController.x()); 106 | } 107 | 108 | @Override 109 | public Trigger getSysIdQuasistaticForward() { 110 | return operatorController.start().and(operatorController.y()); 111 | } 112 | 113 | @Override 114 | public Trigger getSysIdQuasistaticReverse() { 115 | return operatorController.start().and(operatorController.x()); 116 | } 117 | 118 | // Operator Panel 119 | 120 | @Override 121 | public Trigger getVisionIsEnabledTrigger() { 122 | return operatorPanelButtons[10]; 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/sim/ElevatorSystemSim.java: -------------------------------------------------------------------------------- 1 | package frc.lib.team3061.sim; 2 | 3 | import com.ctre.phoenix6.hardware.TalonFX; 4 | import com.ctre.phoenix6.sim.ChassisReference; 5 | import com.ctre.phoenix6.sim.TalonFXSimState; 6 | import edu.wpi.first.math.system.plant.DCMotor; 7 | import edu.wpi.first.wpilibj.RobotController; 8 | import edu.wpi.first.wpilibj.simulation.ElevatorSim; 9 | import edu.wpi.first.wpilibj.util.Color; 10 | import edu.wpi.first.wpilibj.util.Color8Bit; 11 | import frc.robot.Constants; 12 | import org.littletonrobotics.junction.Logger; 13 | import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; 14 | import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; 15 | import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; 16 | 17 | public class ElevatorSystemSim { 18 | 19 | private TalonFX motor; 20 | private TalonFXSimState motorSimState; 21 | private ElevatorSim systemSim; 22 | private double gearRatio; 23 | private double pulleyRadiusMeters; 24 | private String subsystemName; 25 | 26 | // Create a Mechanism2d display of an elevator with a fixed stage and a single extending stage. 27 | private LoggedMechanism2d mech2d; 28 | private LoggedMechanismRoot2d elevatorBase; 29 | private LoggedMechanismLigament2d elevatorExtension; 30 | 31 | public ElevatorSystemSim( 32 | TalonFX motor, 33 | boolean motorInverted, 34 | double gearRatio, 35 | double carriageMassKg, 36 | double pulleyRadiusMeters, 37 | double minHeightMeters, 38 | double maxHeightMeters, 39 | double startingHeightMeters, 40 | String subsystemName) { 41 | if (Constants.getMode() != Constants.Mode.SIM) { 42 | return; 43 | } 44 | 45 | this.motor = motor; 46 | this.gearRatio = gearRatio; 47 | this.pulleyRadiusMeters = pulleyRadiusMeters; 48 | 49 | this.motorSimState = this.motor.getSimState(); 50 | this.motorSimState.Orientation = 51 | motorInverted 52 | ? ChassisReference.Clockwise_Positive 53 | : ChassisReference.CounterClockwise_Positive; 54 | 55 | this.systemSim = 56 | new ElevatorSim( 57 | DCMotor.getKrakenX60Foc(2), 58 | gearRatio, 59 | carriageMassKg, 60 | pulleyRadiusMeters, 61 | minHeightMeters, 62 | maxHeightMeters, 63 | true, 64 | startingHeightMeters); 65 | 66 | this.mech2d = new LoggedMechanism2d(1, 1); 67 | this.elevatorBase = mech2d.getRoot("ElevatorBase", 0.7, 0.1); 68 | this.elevatorExtension = 69 | elevatorBase.append( 70 | new LoggedMechanismLigament2d( 71 | "ElevatorExtension", minHeightMeters, 90.0, 6, new Color8Bit(Color.kYellow))); 72 | 73 | this.subsystemName = subsystemName; 74 | } 75 | 76 | public void updateSim() { 77 | if (Constants.getMode() != Constants.Mode.SIM) { 78 | return; 79 | } 80 | 81 | // update the sim states supply voltage based on the simulated battery 82 | this.motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); 83 | 84 | // update the input voltages of the models based on the outputs of the simulated TalonFXs 85 | double motorVoltage = this.motorSimState.getMotorVoltage(); 86 | this.systemSim.setInput(motorVoltage); 87 | 88 | // update the models 89 | this.systemSim.update(Constants.LOOP_PERIOD_SECS); 90 | 91 | // update the simulated TalonFX based on the model outputs 92 | double mechanismLength = this.systemSim.getPositionMeters(); 93 | double pulleyRotations = mechanismLength / (2 * Math.PI * this.pulleyRadiusMeters); 94 | double motorRotations = pulleyRotations * this.gearRatio; 95 | double mechanismMetersPerSec = this.systemSim.getVelocityMetersPerSecond(); 96 | double pulleyRPS = mechanismMetersPerSec / (2 * Math.PI * this.pulleyRadiusMeters); 97 | double motorRPS = pulleyRPS * this.gearRatio; 98 | this.motorSimState.setRawRotorPosition(motorRotations); 99 | this.motorSimState.setRotorVelocity(motorRPS); 100 | 101 | // Update the Mechanism Arm angle based on the simulated elevator position 102 | this.elevatorExtension.setLength(mechanismLength); 103 | Logger.recordOutput(subsystemName + "/ElevatorSim", mech2d); 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/operator_interface/OISelector.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.robot.operator_interface; 10 | 11 | import edu.wpi.first.wpilibj.Alert; 12 | import edu.wpi.first.wpilibj.Alert.AlertType; 13 | import edu.wpi.first.wpilibj.DriverStation; 14 | import edu.wpi.first.wpilibj2.command.CommandScheduler; 15 | import frc.robot.Constants; 16 | import frc.robot.Constants.RobotType; 17 | 18 | @java.lang.SuppressWarnings({"java:S3776"}) 19 | 20 | /** 21 | * Utility class for selecting the appropriate OI implementations based on the connected joysticks. 22 | */ 23 | public class OISelector { 24 | private static String[] lastJoystickNames = new String[] {null, null, null, null, null, null}; 25 | private static final Alert noOperatorInterfaceWarning = 26 | new Alert("No operator controller(s) connected.", AlertType.kWarning); 27 | private static final Alert nonCompetitionOperatorInterfaceWarning = 28 | new Alert("Non-competition operator controller connected.", AlertType.kWarning); 29 | 30 | private static OperatorInterface oi = null; 31 | 32 | private OISelector() {} 33 | 34 | public static OperatorInterface getOperatorInterface() { 35 | if (didJoysticksChange()) { 36 | CommandScheduler.getInstance().getActiveButtonLoop().clear(); 37 | oi = findOperatorInterface(); 38 | } 39 | return oi; 40 | } 41 | 42 | /** 43 | * Returns whether the connected joysticks have changed since the last time this method was 44 | * called. 45 | */ 46 | private static boolean didJoysticksChange() { 47 | boolean joysticksChanged = false; 48 | for (int port = 0; port < DriverStation.kJoystickPorts; port++) { 49 | String name = DriverStation.getJoystickName(port); 50 | if (!name.equals(lastJoystickNames[port])) { 51 | lastJoystickNames[port] = name; 52 | joysticksChanged = true; 53 | } 54 | } 55 | return joysticksChanged; 56 | } 57 | 58 | /** 59 | * Instantiates and returns an appropriate OperatorInterface object based on the connected 60 | * joysticks. 61 | */ 62 | private static OperatorInterface findOperatorInterface() { 63 | Integer firstPort = null; 64 | Integer secondPort = null; 65 | Integer xBoxPort = null; 66 | Integer thirdPort = null; 67 | for (int port = 0; port < DriverStation.kJoystickPorts; port++) { 68 | if (DriverStation.getJoystickName(port).toLowerCase().contains("xbox")) { 69 | if (xBoxPort == null) { 70 | xBoxPort = port; 71 | } 72 | } else if (!DriverStation.getJoystickName(port).equals("")) { 73 | if (firstPort == null) { 74 | firstPort = port; 75 | } else if (secondPort == null) { 76 | secondPort = port; 77 | } else { // thirdPort == null 78 | thirdPort = port; 79 | } 80 | } 81 | } 82 | 83 | if (Constants.getRobot() == RobotType.ROBOT_SIMBOT 84 | || Constants.getRobot() == RobotType.ROBOT_XRP 85 | || Constants.getRobot() == RobotType.ROBOT_VISION_TEST_PLATFORM) { 86 | nonCompetitionOperatorInterfaceWarning.set(false); 87 | return new SimDualJoysticksOI(0, 1); 88 | } else if (firstPort != null && secondPort != null && xBoxPort != null && thirdPort != null) { 89 | noOperatorInterfaceWarning.set(false); 90 | nonCompetitionOperatorInterfaceWarning.set(true); 91 | return new FullOperatorConsoleOI(firstPort, secondPort, xBoxPort, thirdPort); 92 | } else if (firstPort != null && secondPort != null) { 93 | noOperatorInterfaceWarning.set(false); 94 | nonCompetitionOperatorInterfaceWarning.set(false); 95 | return new DualJoysticksOI(firstPort, secondPort); 96 | } else if (xBoxPort != null) { 97 | noOperatorInterfaceWarning.set(false); 98 | nonCompetitionOperatorInterfaceWarning.set(true); 99 | return new SingleHandheldOI(xBoxPort); 100 | } else { 101 | noOperatorInterfaceWarning.set(true); 102 | nonCompetitionOperatorInterfaceWarning.set(true); 103 | return new OperatorInterface() {}; 104 | } 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team6328/util/LoggedTunableNumber.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 FRC 6328 2 | // http://github.com/Mechanical-Advantage 3 | // 4 | // Use of this source code is governed by an MIT-style 5 | // license that can be found in the LICENSE file at 6 | // the root directory of this project. 7 | 8 | // adapted for 3061-lib 9 | 10 | package frc.lib.team6328.util; 11 | 12 | import static frc.robot.Constants.*; 13 | 14 | import java.util.Arrays; 15 | import java.util.HashMap; 16 | import java.util.Map; 17 | import java.util.function.Consumer; 18 | import java.util.function.DoubleSupplier; 19 | import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; 20 | 21 | /** 22 | * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or 23 | * value not in dashboard. 24 | */ 25 | public class LoggedTunableNumber implements DoubleSupplier { 26 | private static final String TABLE_KEY = "/Tuning"; 27 | 28 | private final String key; 29 | private boolean hasDefault = false; 30 | private double defaultValue; 31 | private LoggedNetworkNumber dashboardNumber; 32 | private Map lastHasChangedValues = new HashMap<>(); 33 | 34 | /** 35 | * Create a new LoggedTunableNumber 36 | * 37 | * @param dashboardKey Key on dashboard 38 | */ 39 | public LoggedTunableNumber(String dashboardKey) { 40 | this.key = TABLE_KEY + "/" + dashboardKey; 41 | } 42 | 43 | /** 44 | * Create a new LoggedTunableNumber with the default value 45 | * 46 | * @param dashboardKey Key on dashboard 47 | * @param defaultValue Default value 48 | */ 49 | public LoggedTunableNumber(String dashboardKey, double defaultValue) { 50 | this(dashboardKey); 51 | initDefault(defaultValue); 52 | } 53 | 54 | /** 55 | * Set the default value of the number. The default value can only be set once. 56 | * 57 | * @param defaultValue The default value 58 | */ 59 | public void initDefault(double defaultValue) { 60 | if (!hasDefault) { 61 | hasDefault = true; 62 | this.defaultValue = defaultValue; 63 | if (TUNING_MODE) { 64 | dashboardNumber = new LoggedNetworkNumber(key, defaultValue); 65 | } 66 | } 67 | } 68 | 69 | /** 70 | * Get the current value, from dashboard if available and in tuning mode. 71 | * 72 | * @return The current value 73 | */ 74 | public double get() { 75 | if (!hasDefault) { 76 | return 0.0; 77 | } else { 78 | return TUNING_MODE ? dashboardNumber.get() : defaultValue; 79 | } 80 | } 81 | 82 | /** 83 | * Checks whether the number has changed since our last check 84 | * 85 | * @param id Unique identifier for the caller to avoid conflicts when shared between multiple 86 | * objects. Recommended approach is to pass the result of "hashCode()" 87 | * @return True if the number has changed since the last time this method was called, false 88 | * otherwise. 89 | */ 90 | public boolean hasChanged(int id) { 91 | double currentValue = get(); 92 | Double lastValue = lastHasChangedValues.get(id); 93 | if (lastValue == null || currentValue != lastValue) { 94 | lastHasChangedValues.put(id, currentValue); 95 | return true; 96 | } 97 | 98 | return false; 99 | } 100 | 101 | /** 102 | * Runs action if any of the tunableNumbers have changed 103 | * 104 | * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * 105 | * objects. Recommended approach is to pass the result of "hashCode()" 106 | * @param action Callback to run when any of the tunable numbers have changed. Access tunable 107 | * numbers in order inputted in method 108 | * @param tunableNumbers All tunable numbers to check 109 | */ 110 | public static void ifChanged( 111 | int id, Consumer action, LoggedTunableNumber... tunableNumbers) { 112 | if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { 113 | action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); 114 | } 115 | } 116 | 117 | /** Runs action if any of the tunableNumbers have changed */ 118 | public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { 119 | ifChanged(id, values -> action.run(), tunableNumbers); 120 | } 121 | 122 | @Override 123 | public double getAsDouble() { 124 | return get(); 125 | } 126 | } 127 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/visualizations/RobotVisualization.java: -------------------------------------------------------------------------------- 1 | package frc.robot.visualizations; 2 | 3 | import static edu.wpi.first.units.Units.*; 4 | 5 | import edu.wpi.first.math.geometry.Pose3d; 6 | import edu.wpi.first.math.geometry.Rotation3d; 7 | import edu.wpi.first.wpilibj.RobotBase; 8 | import edu.wpi.first.wpilibj.util.Color; 9 | import edu.wpi.first.wpilibj.util.Color8Bit; 10 | import frc.lib.team3061.leds.LEDs; 11 | import frc.robot.subsystems.elevator.Elevator; 12 | import org.littletonrobotics.junction.Logger; 13 | import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; 14 | import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; 15 | import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d; 16 | 17 | public class RobotVisualization { 18 | 19 | // Elevator: White 20 | 21 | private Elevator elevator; 22 | 23 | private LoggedMechanism2d visualization2D = new LoggedMechanism2d(1.27, 2.032); 24 | 25 | private LoggedMechanismRoot2d ledDisplayRoot = 26 | visualization2D.getRoot("ledDisplay", 0.58, 1.5); // this goes above the robot 27 | private LoggedMechanismLigament2d ledDisplayBox = 28 | new LoggedMechanismLigament2d("LED State", 0.5, 0, 5.0, new Color8Bit(Color.kBlack)); 29 | 30 | private Color8Bit blue = new Color8Bit(Color.kBlue); 31 | private Color8Bit white = new Color8Bit(Color.kWhite); 32 | 33 | private LoggedMechanismRoot2d driveRoot = visualization2D.getRoot("drive", 0.25, 0); 34 | private LoggedMechanismLigament2d driveLigament = 35 | new LoggedMechanismLigament2d("drivetrain", 0.76, 0.0, 5.0, blue); 36 | 37 | private final double kElevatorPosX = 0.75; 38 | private final double kElevatorPosY = 0.51; 39 | private LoggedMechanismRoot2d elevatorRoot = 40 | visualization2D.getRoot("elevatorRoot", kElevatorPosX, kElevatorPosY); 41 | 42 | private LoggedMechanismLigament2d elevatorLigament = 43 | new LoggedMechanismLigament2d("elevator", 0.13, 90.0, 20.0, white); 44 | 45 | private Pose3d elevatorBasePose3d = new Pose3d(); 46 | private Pose3d elevatorMiddlePose3d = new Pose3d(); 47 | private Pose3d elevatorTopPose3d = new Pose3d(); 48 | private Color8Bit ledColor = new Color8Bit(0, 0, 0); 49 | 50 | public RobotVisualization(Elevator elevator) { 51 | this.elevator = elevator; 52 | if (RobotBase.isReal()) return; 53 | init2dVisualization(); 54 | } 55 | 56 | private void init2dVisualization() { 57 | visualization2D = new LoggedMechanism2d(1.27, 2.032); 58 | 59 | ledDisplayRoot = visualization2D.getRoot("ledDisplay", 0.58, 1.5); // this goes above the robot 60 | ledDisplayBox = 61 | new LoggedMechanismLigament2d("LED State", 0.5, 0, 5.0, new Color8Bit(Color.kBlack)); 62 | 63 | driveRoot = visualization2D.getRoot("drive", 0.25, 0); 64 | driveLigament = new LoggedMechanismLigament2d("drivetrain", 0.76, 0.0, 5.0, blue); 65 | 66 | elevatorRoot = visualization2D.getRoot("elevatorRoot", kElevatorPosX, kElevatorPosY); 67 | 68 | elevatorLigament = new LoggedMechanismLigament2d("elevator", 0.13, 90.0, 20.0, white); 69 | 70 | ledDisplayRoot.append(ledDisplayBox); 71 | driveRoot.append(this.driveLigament); 72 | 73 | // Set up elevator and rollers 74 | elevatorRoot.append(this.elevatorLigament); 75 | } 76 | 77 | public void update() { 78 | // model_0 is elevator bottom cascade 79 | // model_1 is elevator middle cascade 80 | // model_2 is elevator top cascade 81 | double elevatorHeightM = this.elevator.getPosition().in(Meters); 82 | 83 | // Update elevator stages based on height 84 | double baseHeight = 0.0; 85 | double middleHeight = Math.max(0.0, elevatorHeightM - 0.631825); 86 | double topHeight = elevatorHeightM; 87 | 88 | elevatorBasePose3d = new Pose3d(0, 0, baseHeight, new Rotation3d(0, 0, 0)); 89 | elevatorMiddlePose3d = new Pose3d(0, 0, middleHeight, new Rotation3d(0, 0, 0)); 90 | elevatorTopPose3d = new Pose3d(0, 0, topHeight, new Rotation3d(0, 0, 0)); 91 | 92 | Logger.recordOutput( 93 | "Visualization/ComponentsPoseArray", 94 | new Pose3d[] {elevatorBasePose3d, elevatorMiddlePose3d, elevatorTopPose3d}); 95 | 96 | ledColor = LEDs.getInstance().getColor(0); 97 | 98 | if (RobotBase.isReal()) return; 99 | 100 | this.elevatorRoot.setPosition(kElevatorPosX, kElevatorPosY + elevatorHeightM); 101 | 102 | Logger.recordOutput("Visualization/Mechanisms2D", this.visualization2D); 103 | 104 | ledDisplayBox.setColor(ledColor); 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /src/main/java/frc/robot/Region2d.java: -------------------------------------------------------------------------------- 1 | package frc.robot; 2 | 3 | import edu.wpi.first.math.geometry.Pose2d; 4 | import edu.wpi.first.math.geometry.Rotation2d; 5 | import edu.wpi.first.math.geometry.Translation2d; 6 | import java.awt.geom.*; 7 | import java.util.HashMap; 8 | import java.util.Map.Entry; 9 | import java.util.Set; 10 | import org.littletonrobotics.junction.Logger; 11 | 12 | /** 13 | * This class models a region of the field. It is defined by its vertices and the transition points 14 | * to neighboring regions. 15 | */ 16 | public class Region2d { 17 | private Path2D shape; 18 | private HashMap transitionMap; 19 | 20 | /** 21 | * Create a Region2d, a polygon, from an array of Translation2d specifying vertices of a polygon. 22 | * The polygon is created using the even-odd winding rule. 23 | * 24 | * @param points the array of Translation2d that define the vertices of the region. 25 | */ 26 | public Region2d(Translation2d[] points) { 27 | this.transitionMap = new HashMap<>(); 28 | this.shape = new Path2D.Double(Path2D.WIND_EVEN_ODD, points.length); 29 | this.shape.moveTo(points[0].getX(), points[0].getY()); 30 | 31 | for (int i = 1; i < points.length; i++) { 32 | this.shape.lineTo(points[i].getX(), points[i].getY()); 33 | } 34 | 35 | this.shape.closePath(); 36 | } 37 | 38 | /** 39 | * Log the bounding rectangle of the region and the transition points to neighboring regions. 40 | * These can be visualized using AdvantageScope to confirm that the regions are properly defined. 41 | */ 42 | public void logPoints() { 43 | // log the bounding rectangle of the region 44 | Rectangle2D rect = this.shape.getBounds2D(); 45 | Logger.recordOutput("Region2d/point0", new Pose2d(rect.getX(), rect.getY(), new Rotation2d())); 46 | Logger.recordOutput( 47 | "Region2d/point1", 48 | new Pose2d(rect.getX() + rect.getWidth(), rect.getY(), new Rotation2d())); 49 | Logger.recordOutput( 50 | "Region2d/point2", 51 | new Pose2d(rect.getX(), rect.getY() + rect.getHeight(), new Rotation2d())); 52 | Logger.recordOutput( 53 | "Region2d/point3", 54 | new Pose2d( 55 | rect.getX() + rect.getWidth(), rect.getY() + rect.getHeight(), new Rotation2d())); 56 | 57 | // assume that there are at most 4 neighbors 58 | for (int i = 0; i < 4; i++) { 59 | Logger.recordOutput("Region2d/transition" + i, new Pose2d()); 60 | } 61 | int i = 0; 62 | for (Entry entry : transitionMap.entrySet()) { 63 | Translation2d point = entry.getValue(); 64 | Logger.recordOutput( 65 | "Region2d/transition" + i, new Pose2d(point.getX(), point.getY(), new Rotation2d())); 66 | i++; 67 | } 68 | } 69 | 70 | /** 71 | * Returns true if the region contains a given Pose2d. 72 | * 73 | * @param other the given pose2d 74 | * @return if the pose is inside the region 75 | */ 76 | public boolean contains(Pose2d other) { 77 | 78 | return this.shape.contains(new Point2D.Double(other.getX(), other.getY())); 79 | } 80 | 81 | /** 82 | * Add a neighboring Region2d and the ideal point through which to transition from this region to 83 | * the other region. Normally, this method will be invoked once per transition. The transition 84 | * point doesn't need to be on the boundary between the two regions. It may be advantageous for 85 | * the transition point to be located within the other region to influence the generated path. 86 | * Therefore, the transition point from one region to another may not be the same when traversing 87 | * the regions in the opposite direction. 88 | * 89 | * @param other the other region 90 | * @param point a Translation2d representing the transition point 91 | */ 92 | public void addNeighbor(Region2d other, Translation2d point) { 93 | transitionMap.put(other, point); 94 | } 95 | 96 | /** 97 | * Returns a Set of this region's neighbors 98 | * 99 | * @return 100 | */ 101 | public Set getNeighbors() { 102 | return transitionMap.keySet(); 103 | } 104 | 105 | /** 106 | * Get the transition pont between this region and another region. Returns null if they aren't a 107 | * neighbor. 108 | * 109 | * @param other the other region 110 | * @return the transition point, represented as a Translation2d 111 | */ 112 | public Translation2d getTransitionPoint(Region2d other) { 113 | if (getNeighbors().contains(other)) { 114 | 115 | return transitionMap.get(other); 116 | 117 | } else { 118 | return null; 119 | } 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/pneumatics/Pneumatics.java: -------------------------------------------------------------------------------- 1 | // Copyright (c) FIRST and other WPILib contributors. 2 | // Open Source Software; you can modify and/or share it under the terms of 3 | // the WPILib BSD license file in the root directory of this project. 4 | 5 | /* 6 | * Initially from https://github.com/Mechanical-Advantage/RobotCode2022 7 | */ 8 | 9 | package frc.lib.team3061.pneumatics; 10 | 11 | import edu.wpi.first.wpilibj.Alert; 12 | import edu.wpi.first.wpilibj.Alert.AlertType; 13 | import edu.wpi.first.wpilibj.Timer; 14 | import edu.wpi.first.wpilibj2.command.SubsystemBase; 15 | import frc.robot.Constants; 16 | import java.util.ArrayList; 17 | import java.util.List; 18 | import org.littletonrobotics.junction.Logger; 19 | 20 | /** The pneumatics subsystem. */ 21 | public class Pneumatics extends SubsystemBase { 22 | private static final int NORMAL_AVERAGING_TAPS = 25; 23 | private static final int COMPRESSOR_AVERAGING_TAPS = 50; 24 | private static final double COMPRESSOR_RATE_PSI_PER_SEC = 1.5; 25 | 26 | private final PneumaticsIO io; 27 | private final PneumaticsIOInputsAutoLogged inputs = new PneumaticsIOInputsAutoLogged(); 28 | 29 | private final List filterData = new ArrayList<>(); 30 | private double pressureSmoothedPsi = 0.0; 31 | 32 | private double lastPressurePsi = 0.0; 33 | private boolean lastPressureIncreasing = false; 34 | private double compressorMaxPoint = 0.0; 35 | private double compressorMinPoint = 0.0; 36 | 37 | private Timer noPressureTimer = new Timer(); 38 | private Timer compressorEnabledTimer = new Timer(); 39 | private Alert dumpValveAlert = 40 | new Alert("Cannot build pressure. Is the dump value open?", AlertType.kError); 41 | 42 | /** 43 | * Create a new pneumatics subsystem. 44 | * 45 | * @param io the hardware abstracted pneumatics object 46 | */ 47 | public Pneumatics(PneumaticsIO io) { 48 | this.io = io; 49 | noPressureTimer.start(); 50 | compressorEnabledTimer.start(); 51 | } 52 | 53 | public double getPressure() { 54 | return pressureSmoothedPsi; 55 | } 56 | 57 | @Override 58 | public void periodic() { 59 | io.updateInputs(inputs); 60 | Logger.processInputs("Pneumatics", inputs); 61 | 62 | calculateAveragePressure(); 63 | 64 | // Log pressure 65 | Logger.recordOutput("PressurePsi", pressureSmoothedPsi); 66 | 67 | // Detect if dump value is open 68 | if (inputs.highPressurePSI > 3) { 69 | noPressureTimer.reset(); 70 | } 71 | if (!inputs.compressorActive) { 72 | compressorEnabledTimer.reset(); 73 | } 74 | dumpValveAlert.set(noPressureTimer.hasElapsed(5) && compressorEnabledTimer.hasElapsed(5)); 75 | } 76 | 77 | private void calculateAveragePressure() { 78 | // Calculate input pressure for averaging filter 79 | double limitedPressure = inputs.highPressurePSI < 0.0 ? 0.0 : inputs.highPressurePSI; 80 | double processedPressure; 81 | if (inputs.compressorActive) { 82 | // When compressor is active, average the most recent min and max points 83 | processedPressure = averagePressures(limitedPressure); 84 | } else { 85 | // When compressor is inactive, reset min/max status and use normal pressure 86 | processedPressure = resetPressures(limitedPressure); 87 | } 88 | 89 | // Run averaging filter 90 | filterData.add(processedPressure); 91 | while (filterData.size() 92 | > (inputs.compressorActive ? COMPRESSOR_AVERAGING_TAPS : NORMAL_AVERAGING_TAPS)) { 93 | filterData.remove(0); 94 | } 95 | pressureSmoothedPsi = filterData.stream().mapToDouble(a -> a).summaryStatistics().getAverage(); 96 | } 97 | 98 | private double averagePressures(double limitedPressure) { 99 | if (limitedPressure != lastPressurePsi) { 100 | boolean increasing = limitedPressure > lastPressurePsi; 101 | if (increasing != lastPressureIncreasing) { 102 | if (increasing) { 103 | compressorMinPoint = lastPressurePsi; 104 | } else { 105 | compressorMaxPoint = lastPressurePsi; 106 | } 107 | } 108 | lastPressurePsi = limitedPressure; 109 | lastPressureIncreasing = increasing; 110 | } 111 | double processedPressure = (compressorMinPoint + compressorMaxPoint) / 2.0; 112 | 113 | // Apply latency compensation 114 | processedPressure += 115 | (COMPRESSOR_AVERAGING_TAPS / 2.0) 116 | * Constants.LOOP_PERIOD_SECS 117 | * COMPRESSOR_RATE_PSI_PER_SEC; 118 | 119 | return processedPressure; 120 | } 121 | 122 | private double resetPressures(double limitedPressure) { 123 | lastPressurePsi = limitedPressure; 124 | lastPressureIncreasing = false; 125 | compressorMaxPoint = limitedPressure; 126 | compressorMinPoint = limitedPressure; 127 | return limitedPressure; 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /src/main/java/frc/lib/team3061/gyro/GyroIOPigeon2Phoenix6.java: -------------------------------------------------------------------------------- 1 | /* 2 | * Initially from https://github.com/Mechanical-Advantage/SwerveDevelopment 3 | */ 4 | 5 | package frc.lib.team3061.gyro; 6 | 7 | import static edu.wpi.first.units.Units.*; 8 | 9 | import com.ctre.phoenix6.BaseStatusSignal; 10 | import com.ctre.phoenix6.StatusSignal; 11 | import com.ctre.phoenix6.hardware.Pigeon2; 12 | import com.ctre.phoenix6.sim.Pigeon2SimState; 13 | import edu.wpi.first.math.filter.Debouncer; 14 | import edu.wpi.first.units.measure.Angle; 15 | import edu.wpi.first.units.measure.AngularVelocity; 16 | import edu.wpi.first.wpilibj.RobotController; 17 | import frc.lib.team254.Phoenix6Util; 18 | import frc.lib.team3061.RobotConfig; 19 | import frc.robot.Constants; 20 | 21 | public class GyroIOPigeon2Phoenix6 implements GyroIO { 22 | 23 | private static final double SIGNAL_UPDATE_FREQUENCY_HZ = 100.0; 24 | 25 | private final Pigeon2 gyro; 26 | private final StatusSignal yawStatusSignal; 27 | private final StatusSignal pitchStatusSignal; 28 | private final StatusSignal rollStatusSignal; 29 | private final StatusSignal angularVelocityXStatusSignal; 30 | private final StatusSignal angularVelocityYStatusSignal; 31 | private final StatusSignal angularVelocityZStatusSignal; 32 | 33 | private final Debouncer connectedDebouncer = new Debouncer(0.5); 34 | 35 | private final Pigeon2SimState gyroSim; 36 | 37 | public GyroIOPigeon2Phoenix6(int id) { 38 | gyro = new Pigeon2(id, RobotConfig.getInstance().getCANBusName()); 39 | this.yawStatusSignal = this.gyro.getYaw(); 40 | this.yawStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 41 | this.pitchStatusSignal = this.gyro.getPitch(); 42 | this.pitchStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 43 | this.rollStatusSignal = this.gyro.getRoll(); 44 | this.rollStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 45 | this.angularVelocityXStatusSignal = this.gyro.getAngularVelocityXWorld(); 46 | this.angularVelocityXStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 47 | this.angularVelocityYStatusSignal = this.gyro.getAngularVelocityYWorld(); 48 | this.angularVelocityYStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 49 | this.angularVelocityZStatusSignal = this.gyro.getAngularVelocityZWorld(); 50 | this.angularVelocityZStatusSignal.setUpdateFrequency(SIGNAL_UPDATE_FREQUENCY_HZ); 51 | 52 | // To improve performance, register all signals with Phoenix6Util. All signals on the entire CAN 53 | // bus will be refreshed at the same time by Phoenix6Util; so, there is no need to refresh any 54 | // StatusSignals in this class. 55 | Phoenix6Util.registerSignals( 56 | true, 57 | this.yawStatusSignal, 58 | this.angularVelocityZStatusSignal, 59 | this.pitchStatusSignal, 60 | this.rollStatusSignal, 61 | this.angularVelocityXStatusSignal, 62 | this.angularVelocityYStatusSignal); 63 | 64 | if (Constants.getMode() == Constants.Mode.SIM) { 65 | this.gyroSim = this.gyro.getSimState(); 66 | } else { 67 | this.gyroSim = null; 68 | } 69 | } 70 | 71 | @Override 72 | public void updateInputs(GyroIOInputs inputs) { 73 | // Determine if the Pigeon is still connected (i.e., reachable on the CAN bus). We do this by 74 | // verifying that none of the status signals for the device report an error. 75 | inputs.connected = 76 | connectedDebouncer.calculate( 77 | BaseStatusSignal.isAllGood( 78 | this.yawStatusSignal, 79 | this.angularVelocityZStatusSignal, 80 | this.pitchStatusSignal, 81 | this.rollStatusSignal, 82 | this.angularVelocityXStatusSignal, 83 | this.angularVelocityYStatusSignal)); 84 | inputs.yawDeg = 85 | BaseStatusSignal.getLatencyCompensatedValue( 86 | this.yawStatusSignal, this.angularVelocityZStatusSignal) 87 | .in(Degrees); 88 | inputs.pitchDeg = 89 | BaseStatusSignal.getLatencyCompensatedValue( 90 | this.pitchStatusSignal, this.angularVelocityYStatusSignal) 91 | .in(Degrees); 92 | inputs.rollDeg = 93 | BaseStatusSignal.getLatencyCompensatedValue( 94 | this.rollStatusSignal, this.angularVelocityXStatusSignal) 95 | .in(Degrees); 96 | inputs.rollDegPerSec = this.angularVelocityXStatusSignal.getValue().in(DegreesPerSecond); 97 | inputs.pitchDegPerSec = this.angularVelocityYStatusSignal.getValue().in(DegreesPerSecond); 98 | inputs.yawDegPerSec = this.angularVelocityZStatusSignal.getValue().in(DegreesPerSecond); 99 | 100 | // The last step in the updateInputs method is to update the simulation. 101 | if (Constants.getMode() == Constants.Mode.SIM) { 102 | this.gyroSim.setSupplyVoltage(RobotController.getBatteryVoltage()); 103 | } 104 | } 105 | } 106 | --------------------------------------------------------------------------------