From bf4da44338d8e91c50e34c3ac68dc12f9d335b08 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 11 Jul 2025 14:07:53 -0600 Subject: [PATCH 01/10] The great code restructuring --- .vscode/launch.json | 12 +- build.gradle | 22 +- simgui-ds.json | 17 +- src/main/java/frc4388/robot/Constants.java | 472 ------------------ src/main/java/frc4388/robot/Robot.java | 132 ++++- .../java/frc4388/robot/RobotContainer.java | 136 ++--- src/main/java/frc4388/robot/RobotMap.java | 74 ++- .../robot/commands/MoveForTimeCommand.java | 4 +- .../robot/commands/MoveUntilSuply.java | 2 - src/main/java/frc4388/robot/commands/PID.java | 2 +- .../robot/commands/WhileTrueCommand.java | 1 - .../DriveToReef.java} | 83 +-- .../{ => alignment}/DriveUntilLiDAR.java | 10 +- .../commands/{ => alignment}/LidarAlign.java | 4 +- .../neo AutoRecoding format.txt | 0 .../{Autos => autos}/neoPlaybackChooser.java | 1 + .../{Swerve => swerve}/RotateToAngle.java | 2 +- .../neoJoystickPlayback.java | 16 +- .../neoJoystickRecorder.java | 8 +- .../{ => wait}/waitElevatorRefrence.java | 2 +- .../{ => wait}/waitEndefectorRefrence.java | 2 +- .../commands/{ => wait}/waitFeedCoral.java | 2 +- .../commands/{ => wait}/waitSupplier.java | 2 +- .../robot/constants/BuildConstants.java | 19 + .../frc4388/robot/constants/Constants.java | 234 +++++++++ .../robot/constants/DriveConstants.java | 247 +++++++++ .../frc4388/robot/subsystems/DiffDrive.java | 9 +- .../frc4388/robot/subsystems/Elevator.java | 28 +- .../java/frc4388/robot/subsystems/LED.java | 14 +- .../java/frc4388/robot/subsystems/Lidar.java | 9 +- .../frc4388/robot/subsystems/SwerveDrive.java | 142 +++--- .../java/frc4388/robot/subsystems/Vision.java | 66 +-- src/main/java/frc4388/utility/Alliance.java | 5 - src/main/java/frc4388/utility/AprilTag.java | 13 - src/main/java/frc4388/utility/RobotGyro.java | 2 + src/main/java/frc4388/utility/Trim.java | 63 ++- .../utility/{ => compute}/DataUtils.java | 2 +- .../{ => compute}/ReefPositionHelper.java | 10 +- .../utility/{ => compute}/RobotTime.java | 2 +- .../utility/{ => compute}/RobotUnits.java | 2 +- .../{ => compute}/TimesNegativeOne.java | 20 +- .../controller/DeadbandedXboxController.java | 2 +- .../utility/{ => status}/CanDevice.java | 8 +- .../frc4388/utility/{ => status}/Status.java | 2 +- .../utility/{ => status}/Subsystem.java | 2 +- .../frc4388/utility/{ => structs}/Gains.java | 2 +- .../utility/{ => structs}/LEDPatterns.java | 2 +- .../utility/{ => structs}/UtilityStructs.java | 2 +- vendordeps/AdvantageKit.json | 35 ++ ....2.4.json => PathplannerLib-2025.2.7.json} | 8 +- ....1.0.json => Phoenix6-frc2025-latest.json} | 144 +++++- vendordeps/photonlib.json | 12 +- 52 files changed, 1142 insertions(+), 970 deletions(-) delete mode 100644 src/main/java/frc4388/robot/Constants.java rename src/main/java/frc4388/robot/commands/{GotoLastApril.java => alignment/DriveToReef.java} (70%) rename src/main/java/frc4388/robot/commands/{ => alignment}/DriveUntilLiDAR.java (83%) rename src/main/java/frc4388/robot/commands/{ => alignment}/LidarAlign.java (97%) rename src/main/java/frc4388/robot/commands/{Autos => autos}/neo AutoRecoding format.txt (100%) rename src/main/java/frc4388/robot/commands/{Autos => autos}/neoPlaybackChooser.java (99%) rename src/main/java/frc4388/robot/commands/{Swerve => swerve}/RotateToAngle.java (95%) rename src/main/java/frc4388/robot/commands/{Swerve => swerve}/neoJoystickPlayback.java (94%) rename src/main/java/frc4388/robot/commands/{Swerve => swerve}/neoJoystickRecorder.java (96%) rename src/main/java/frc4388/robot/commands/{ => wait}/waitElevatorRefrence.java (97%) rename src/main/java/frc4388/robot/commands/{ => wait}/waitEndefectorRefrence.java (97%) rename src/main/java/frc4388/robot/commands/{ => wait}/waitFeedCoral.java (96%) rename src/main/java/frc4388/robot/commands/{ => wait}/waitSupplier.java (96%) create mode 100644 src/main/java/frc4388/robot/constants/BuildConstants.java create mode 100644 src/main/java/frc4388/robot/constants/Constants.java create mode 100644 src/main/java/frc4388/robot/constants/DriveConstants.java delete mode 100644 src/main/java/frc4388/utility/Alliance.java delete mode 100644 src/main/java/frc4388/utility/AprilTag.java rename src/main/java/frc4388/utility/{ => compute}/DataUtils.java (96%) rename src/main/java/frc4388/utility/{ => compute}/ReefPositionHelper.java (93%) rename src/main/java/frc4388/utility/{ => compute}/RobotTime.java (98%) rename src/main/java/frc4388/utility/{ => compute}/RobotUnits.java (96%) rename src/main/java/frc4388/utility/{ => compute}/TimesNegativeOne.java (68%) rename src/main/java/frc4388/utility/{ => status}/CanDevice.java (83%) rename src/main/java/frc4388/utility/{ => status}/Status.java (99%) rename src/main/java/frc4388/utility/{ => status}/Subsystem.java (95%) rename src/main/java/frc4388/utility/{ => structs}/Gains.java (98%) rename src/main/java/frc4388/utility/{ => structs}/LEDPatterns.java (98%) rename src/main/java/frc4388/utility/{ => structs}/UtilityStructs.java (95%) create mode 100644 vendordeps/AdvantageKit.json rename vendordeps/{PathplannerLib-2025.2.4.json => PathplannerLib-2025.2.7.json} (87%) rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-frc2025-latest.json} (75%) diff --git a/.vscode/launch.json b/.vscode/launch.json index 5b804e8..fb77e69 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc4388.robot.Main", + "projectName": "2025RidgeScape" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/build.gradle b/build.gradle index 689ff84..e8723ce 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "com.peterabeles.gversion" version "1.10" } java { @@ -72,6 +73,9 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } test { @@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + +// Create version file +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc4388.robot.constants" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Denver" + indent = " " +} diff --git a/simgui-ds.json b/simgui-ds.json index 00784de..14c4c68 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -96,7 +91,17 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "030000005e040000ea0200000b050000", + "useGamepad": true + }, + { + "useGamepad": true + }, + {}, + {}, + {}, + { + "useGamepad": true } ] } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java deleted file mode 100644 index 7f0cbfa..0000000 --- a/src/main/java/frc4388/robot/Constants.java +++ /dev/null @@ -1,472 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot; - -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Rotations; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; - -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; -import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; -import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import frc4388.utility.CanDevice; -import frc4388.utility.Gains; -import frc4388.utility.LEDPatterns; -import frc4388.utility.ReefPositionHelper; -import frc4388.utility.Trim; -import frc4388.utility.configurable.ConfigurableDouble; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be - * declared globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final String CANBUS_NAME = "rio"; - - public static final class SwerveDriveConstants { - - public static final double MAX_ROT_SPEED = Math.PI * 2; - public static final double AUTO_MAX_ROT_SPEED = 1.5; - public static final double MIN_ROT_SPEED = 1.0; - public static double ROTATION_SPEED = MAX_ROT_SPEED; - public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; - public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; - - public static final double CORRECTION_MIN = 10; - public static final double CORRECTION_MAX = 50; - - public static final double SLOW_SPEED = 0.25; - public static final double FAST_SPEED = 0.5; - public static final double TURBO_SPEED = 1.0; - - public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; - public static final int STARTING_GEAR = 0; - // Dimensions - public static final double WIDTH = 18.5; // TODO: validate - public static final double HEIGHT = 18.5; // TODO: validate - public static final double HALF_WIDTH = WIDTH / 2.d; - public static final double HALF_HEIGHT = HEIGHT / 2.d; - - // Mechanics - private static final double COUPLE_RATIO = 3; //todo: find - private static final double DRIVE_RATIO = 6.12; - private static final double STEER_RATIO = (150/7); - private static final Distance WHEEL_RADIUS = Inches.of(2); - - public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate - - public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; - public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; - - // Operation - public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 - - public static final boolean DRIFT_CORRECTION_ENABLED = true; - public static final boolean INVERT_X = false; - public static final boolean INVERT_Y = true; - public static final boolean INVERT_ROTATION = false; - - // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); - - private static final class ModuleSpecificConstants { //2025 - //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); - private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; - private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); - private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; - private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; - private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); - private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - - //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); - private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_LEFT_ENCODER_INVERTED = false; - private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); - - //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); - private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; - private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; - private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; - private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); - private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); - } - - public static final class IDs { - public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); - public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); - public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11); - - public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2); - public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3); - public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10); - - public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6); - public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7); - public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12); - - public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8); - public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9); - public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); - - public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); - } - - public static final class PIDConstants { - public static final int SWERVE_SLOT_IDX = 0; - public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0); - - public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() - .withKP(50).withKI(0).withKD(0.32) - .withKS(0).withKV(0).withKA(0); - - public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs() - .withKP(10).withKI(0).withKD(0.3) - .withKS(0).withKV(0).withKA(0); - - public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.6) - .withKS(0.1).withKV(1.91).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); - public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); - } - - public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. - public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. - public static final double NEUTRAL_DEADBAND = 0.04; - - // POWER! (limiting) - private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - ).withOpenLoopRamps( - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - ).withClosedLoopRamps( - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ); - private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND) - // ).withOpenLoopRamps( - // new OpenLoopRampsConfigs() - // .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) - // ).withClosedLoopRamps( - // new ClosedLoopRampsConfigs() - // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) - ); - public static final double SLIP_CURRENT = 60; // TODO: Tune??? - } - - // No mans land - // Beware, there be dragons. - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() // holy verbosity batman. - .withDriveMotorGearRatio(DRIVE_RATIO) - .withSteerMotorGearRatio(STEER_RATIO) - .withCouplingGearRatio(COUPLE_RATIO) - .withWheelRadius(WHEEL_RADIUS) - .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? - .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? - .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) - .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) - .withSlipCurrent(Configurations.SLIP_CURRENT) - .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC) - .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) - .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) - .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) - .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS) - .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS); - - public static final SwerveModuleConstants FRONT_LEFT = - ConstantCreator.createModuleConstants( - IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET, - ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, - ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED - ); - public static final SwerveModuleConstants FRONT_RIGHT = - ConstantCreator.createModuleConstants( - IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET, - ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, - ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED - ); - public static final SwerveModuleConstants BACK_LEFT = - ConstantCreator.createModuleConstants( - IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET, - ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, - ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED - ); - public static final SwerveModuleConstants BACK_RIGHT = - ConstantCreator.createModuleConstants( - IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET, - ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, - ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED - ); - - // misc - public static final int TIMEOUT_MS = 30; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class LiDARConstants { - public static final int REEF_LIDAR_DIO_CHANNEL = 7; - public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; - - public static final int HUMAN_PLAYER_STATION_DISTANCE = 40; - - public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - public static final int LIDAR_MICROS_TO_CM = 10; - public static final int SECONDS_TO_MICROS = 1000000; - } - - public static final class AutoConstants { - // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); - public static final Gains XY_GAINS = new Gains(8,0,0.0); - // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); - // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); - // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); - // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); - - // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); - // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); - - public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); - // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); - public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); - public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); - public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); - - public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 5; // Degrees - - public static final double MIN_XY_PID_OUTPUT = 0.0; - public static final double MIN_ROT_PID_OUTPUT = 0.0; - - public static final double VELOCITY_THRESHHOLD = 0.01; - - // X is tangent to reef side - // Y is normal to reef side - public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field - public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); - public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); - - public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); - public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; - // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); - - public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); - public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); - - public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); - public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2); - - public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; - - // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); - // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); - - // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); - // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); - - // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); - // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); - - // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); - - public static final int L4_DRIVE_TIME = 250; //Milliseconds - // public static final int L3_DRIVE_TIME = 500; - public static final int L2_DRIVE_TIME = 250; //Milliseconds - public static final int ALGAE_DRIVE_TIME = 500; - public static final double STOP_VELOCITY = 0.0; - } - - public static final class VisionConstants { - public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; - public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; - - public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); - - public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters - - // Photonvision thing - // The standard deviations of our vision estimated poses, which affect correction rate - // X, Y, Theta - // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); - } - - public static final class FieldConstants { - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - - // Test april tag field layout - // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - // Arrays.asList(new AprilTag[] { - // new AprilTag(1, new Pose3d( - // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - // )), - // }), 100, 100); - - } - - public static final class DriveConstants { - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - - public static final class LEDConstants { - public static final int LED_SPARK_ID = 9; - - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - - public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; - public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; - public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; - public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; - - public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; - public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; - } - - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; - public static final int BUTTONBOX_ID = 2; - public static final int XBOX_PROGRAMMER_ID = 3; - public static final double LEFT_AXIS_DEADBAND = 0.1; - - } - - public static final class ElevatorConstants { - public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); - public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); - - public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75; - public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20; - - // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND - - public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND - public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND - public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND - - - public static final double GEAR_RATIO_ELEVATOR = -9.0; - //Max for elevator = 50% - - public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; - public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe - public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe - public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe - public static final double SCORING_THREE_ELEVATOR = -9.25; - public static final double DEALGAE_L2_ELEVATOR = -4; - public static final double DEALGAE_L3_ELEVATOR = -26.5; - public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position - - public static final double GEAR_RATIO_ENDEFECTOR = -100.0; - public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; - //Max for endefector = 60% - public static final double L2_SCORE_ELEVATOR = -5; - public static final double L2_LEAVE_ELEVATOR = -11; - - public static final double L2_SCORE_ENDEFFECTOR = -19; - - public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; - public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; - public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; - public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; - public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; - public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; - - public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() - .withKP(1) - .withKI(0) - .withKD(0); - - public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs() - .withKP(1) - .withKI(0) - .withKD(0); - } -} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 092de15..6b948bf 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -11,24 +11,31 @@ import java.util.ArrayList; import java.util.Base64; import java.util.List; +// Advantagekit +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc4388.utility.CanDevice; +import frc4388.robot.constants.BuildConstants; +import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; import frc4388.utility.DeferredBlockMulti; -import frc4388.utility.RobotTime; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; import frc4388.utility.Trim; -import frc4388.utility.Status.Report; -import frc4388.utility.Status.ReportLevel; +import frc4388.utility.compute.RobotTime; +import frc4388.utility.status.CanDevice; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.Report; +import frc4388.utility.status.Status.ReportLevel; //import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the @@ -37,7 +44,7 @@ import frc4388.utility.Status.ReportLevel; * creating this project, you must also update the build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { Command m_autonomousCommand; private RobotTime m_robotTime = RobotTime.getInstance(); @@ -55,7 +62,7 @@ public class Robot extends TimedRobot { m_robotContainer = new RobotContainer(); - + // Create a shuffleboard update thread, that will periodically update the values on shuffleboard new Thread() { public void run() { try{ @@ -69,7 +76,7 @@ public class Robot extends TimedRobot { } }catch(Exception e){ - e.printStackTrace(); + e.printStackTrace(); } } }.start(); @@ -248,4 +255,101 @@ public class Robot extends TimedRobot { } } -} + + + + + + + + // VisionSystemSim visionSim; + // RobotMapSim robotMapSim; + + // @Override + // public void simulationInit() { + // visionSim = new VisionSystemSim("main"); + // robotMapSim = m_robotContainer.m_robotMap.configureSim(); + + + // // A 0.5 x 0.25 meter rectangular target + // TargetModel targetModel = new TargetModel(0.5, 0.25); + // // The pose of where the target is on the field. + // // Its rotation determines where "forward" or the target x-axis points. + // // Let's say this target is flat against the far wall center, facing the blue driver stations. + // Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI)); + // // The given target model at the given pose + // VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel); + + // // Add this vision target to the vision system simulation to make it visible + // visionSim.addVisionTargets(visionTarget); + + // // The layout of AprilTags which we want to add to the vision system + // AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout; + + // visionSim.addAprilTags(tagLayout); + + + // visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS); + // visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS); + + // SmartDashboard.putData(visionSim.getDebugField()); + // } + + + // @Override + // public void simulationPeriodic() { + // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); + // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); + + // // m_robotContainer.m_robotSwerveDrive. + // } + + + +// Start AdvantageKit logging / replay and record metadata +// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java + public void startLogging() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + // Set up data receivers & replay source + switch (SimConstants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick ("/U/logs") + Logger.addDataReceiver(new WPILOGWriter()); + Logger.addDataReceiver(new NT4Publisher()); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + // Start AdvantageKit logger + Logger.start(); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37a6cbb..ae0ae64 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,34 +11,21 @@ package frc4388.robot; import edu.wpi.first.wpilibj.DriverStation; import java.io.File; -import java.util.ArrayList; -import java.util.List; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.robot.Constants.FieldConstants; -import frc4388.robot.Constants.LiDARConstants; -import frc4388.robot.Constants.OIConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.AutoConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -47,20 +34,19 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; // Autos import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.DriveUntilLiDAR; -import frc4388.robot.commands.GotoLastApril; -import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.MoveUntilSuply; -import frc4388.robot.commands.WhileTrueCommand; -import frc4388.robot.commands.waitElevatorRefrence; -import frc4388.robot.commands.waitEndefectorRefrence; -import frc4388.robot.commands.waitFeedCoral; -import frc4388.robot.commands.waitSupplier; -import frc4388.robot.commands.Swerve.neoJoystickPlayback; -import frc4388.robot.commands.Swerve.neoJoystickRecorder; - -import com.pathplanner.lib.auto.AutoBuilder; +import frc4388.robot.commands.alignment.DriveToReef; +import frc4388.robot.commands.alignment.DriveUntilLiDAR; +import frc4388.robot.commands.alignment.LidarAlign; +import frc4388.robot.commands.wait.waitElevatorRefrence; +import frc4388.robot.commands.wait.waitEndefectorRefrence; +import frc4388.robot.commands.wait.waitFeedCoral; +import frc4388.robot.commands.wait.waitSupplier; +import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.OIConstants; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -68,7 +54,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.Elevator; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; @@ -76,11 +61,8 @@ import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; import frc4388.utility.DeferredBlockMulti; -import frc4388.utility.ReefPositionHelper; -import frc4388.utility.Subsystem; -import frc4388.utility.TimesNegativeOne; -import frc4388.utility.ReefPositionHelper.Side; -import frc4388.utility.configurable.ConfigurableString; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.compute.ReefPositionHelper.Side; /** * This class is where the bulk of the robot should be declared. Since @@ -97,8 +79,6 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); - public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -108,11 +88,6 @@ public class RobotContainer { private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); - private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID); - - /* Virtual Controllers */ - private final VirtualController m_virtualDriver = new VirtualController(0); - private final VirtualController m_virtualOperator = new VirtualController(1); // public List subsystems = new ArrayList<>(); @@ -124,7 +99,6 @@ public class RobotContainer { } // ! /* Autos */ - private String lastAutoName = "defualt.auto"; private SendableChooser autoChooser; private Command autoCommand; @@ -144,7 +118,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -153,9 +127,9 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -186,7 +160,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -195,9 +169,9 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -243,7 +217,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -279,7 +253,7 @@ public class RobotContainer { new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), @@ -307,7 +281,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -316,9 +290,9 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -354,7 +328,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL4Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), @@ -363,9 +337,9 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -398,15 +372,15 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true) ), () -> m_robotElevator.isL3Primed()), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -423,7 +397,7 @@ public class RobotContainer { new ConditionalCommand(Commands.none(), new SequentialCommandGroup( // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true) + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true) ),() -> m_robotElevator.isL3Primed()), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), @@ -432,10 +406,10 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -450,12 +424,12 @@ public class RobotContainer { private Command AprilLidarAlignL2Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true), waitDebuger.asProxy(), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -471,12 +445,12 @@ public class RobotContainer { private Command AprilLidarAlignL2Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true), waitDebuger.asProxy(), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), + new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -495,7 +469,7 @@ public class RobotContainer { // new waitEndefectorRefrence(m_robotElevator), // new waitElevatorRefrence(m_robotElevator), // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), @@ -506,10 +480,10 @@ public class RobotContainer { private Command lowerAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), waitDebuger.asProxy(), // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, @@ -526,7 +500,7 @@ public class RobotContainer { // new waitEndefectorRefrence(m_robotElevator), // new waitElevatorRefrence(m_robotElevator), // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), // new MoveForTimeCommand(m_robotSwerveDrive, // new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), @@ -536,10 +510,10 @@ public class RobotContainer { private Command upperAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), waitDebuger.asProxy(), // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, @@ -584,7 +558,7 @@ public class RobotContainer { NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); @@ -688,7 +662,7 @@ public class RobotContainer { private void configureButtonBindings() { // ? /* Driver Buttons */ - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // ! /* Speed */ @@ -739,21 +713,21 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, - // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); .onTrue(WannaSeeMeDunk.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_robotMap.reefLidar)); // ? /* Operator Buttons */ - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); // Button box @@ -966,12 +940,4 @@ public class RobotContainer { public ButtonBox getButtonBox() { return this.m_buttonBox; } - - public VirtualController getVirtualDriverController() { - return m_virtualDriver; - } - - public VirtualController getVirtualOperatorController() { - return m_virtualOperator; - } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 577f6bf..a306f43 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,22 +10,18 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; import org.photonvision.PhotonCamera; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; - +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DigitalInput; -import frc4388.robot.Constants.ElevatorConstants; -// import edu.wpi.first.wpilibj.motorcontrol.Spark; -// import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.VisionConstants; -// import frc4388.robot.subsystems.SwerveModule; -import frc4388.utility.RobotGyro; +import frc4388.robot.constants.Constants.ElevatorConstants; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.Lidar; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -35,8 +31,12 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + public final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + + public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); + public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); + public RobotMap() { configureDriveMotorControllers(); @@ -47,9 +47,9 @@ public class RobotMap { /* Swreve Drive Subsystem */ public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - Constants.SwerveDriveConstants.DrivetrainConstants, - Constants.SwerveDriveConstants.FRONT_LEFT, Constants.SwerveDriveConstants.FRONT_RIGHT, - Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT + DriveConstants.DrivetrainConstants, + DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT, + DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT ); /* Elevator Subsystem */ @@ -64,5 +64,43 @@ public class RobotMap { void configureDriveMotorControllers() { // endeffector.saf } + + + public class RobotMapSim { + public PhotonCameraSim leftCamera; + public PhotonCameraSim rightCamera; + } + + public RobotMapSim configureSim() { + RobotMapSim sim = new RobotMapSim(); + + // The simulated camera properties + SimCameraProperties cameraProp = new SimCameraProperties(); + // A 640 x 480 camera with a 100 degree diagonal FOV. + cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); + // Approximate detection noise with average and standard deviation error in pixels. + cameraProp.setCalibError(0.25, 0.08); + // Set the camera image capture framerate (Note: this is limited by robot loop rate). + cameraProp.setFPS(20); + // The average and standard deviation in milliseconds of image data latency. + cameraProp.setAvgLatencyMs(35); + cameraProp.setLatencyStdDevMs(5); + + sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + -} + sim.leftCamera.enableRawStream(true); + sim.leftCamera.enableProcessedStream(true); + sim.leftCamera.enableDrawWireframe(true); + + + sim.rightCamera.enableRawStream(true); + sim.rightCamera.enableProcessedStream(true); + sim.rightCamera.enableDrawWireframe(true); + + return sim; + + } + +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index fdbb619..6a085ec 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -5,7 +5,6 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.TimesNegativeOne; // Command to repeat a joystick movement for a specific time. public class MoveForTimeCommand extends Command { @@ -23,7 +22,8 @@ public class MoveForTimeCommand extends Command { Translation2d rightStick, long millis, boolean robotRelative) { - addRequirements(swerveDrive); + + addRequirements(swerveDrive); this.swerveDrive = swerveDrive; this.leftStick = leftStick; diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java index dd0d3e2..aa1b210 100644 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -1,12 +1,10 @@ package frc4388.robot.commands; -import java.time.Instant; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.TimesNegativeOne; // Command to repeat a joystick movement for a specific time. public class MoveUntilSuply extends Command { diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java index 97233f8..c95c9fd 100644 --- a/src/main/java/frc4388/robot/commands/PID.java +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -5,7 +5,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.utility.Gains; +import frc4388.utility.structs.Gains; public abstract class PID extends Command { protected Gains gains; diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java index 8db5f64..2a0d045 100644 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java @@ -8,7 +8,6 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java similarity index 70% rename from src/main/java/frc4388/robot/commands/GotoLastApril.java rename to src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 0686440..ac59724 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -1,29 +1,19 @@ -package frc4388.robot.commands; - -import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; - -import java.util.Optional; +package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.Constants.AutoConstants; +import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; -import frc4388.utility.Gains; -import frc4388.utility.ReefPositionHelper; -import frc4388.utility.TimesNegativeOne; -import frc4388.utility.ReefPositionHelper.Side; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; -import frc4388.utility.controller.VirtualController; +import frc4388.utility.compute.ReefPositionHelper; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.compute.ReefPositionHelper.Side; +import frc4388.utility.structs.Gains; -public class GotoLastApril extends Command { +public class DriveToReef extends Command { // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); @@ -45,7 +35,7 @@ public class GotoLastApril extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { + public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) { this.swerveDrive = swerveDrive; this.vision = vision; this.distance = distance; @@ -62,23 +52,6 @@ public class GotoLastApril extends Command { @Override public void initialize() { - // double kP = AutoConstants.P_XY_GAINS.get(); - // double kI = AutoConstants.I_XY_GAINS.get(); - // double kD = AutoConstants.D_XY_GAINS.get(); - // xPID = new PID(new Gains( - // kP, - // kI, - // kD), - // 0); - // yPID = new PID(new Gains( - // kP, - // kI, - // kD), - // 0); - - // System.out.println("kP: "+kP); - // System.out.println("kI: "+kI); - // System.out.println("kD: "+kD); xPID.initialize(); yPID.initialize(); this.targetpos = ReefPositionHelper.getNearestPosition( @@ -168,33 +141,6 @@ public class GotoLastApril extends Command { return finished; // this statement is a boolean in and of itself } - - // @Override - // public void end(boolean interrupted) { - - // } - - - - // @Override - // public double getError() { - // return e; - // } - - - - // @Override - // public void runWithOutput(double output) { - // // TODO Auto-generated method stub - // Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0); - // Translation2d rightStick = new Translation2d(); - // // System.out.println("Output = " + -output); - // SmartDashboard.putNumber("PID Output", output); - // swerveDrive.driveWithInput(leftStick, rightStick, true); - // } - - - @@ -212,17 +158,11 @@ public class GotoLastApril extends Command { private class PID { protected Gains gains; private double output = 0; - private double tolerance = 0; + /** Creates a new PelvicInflammatoryDisease. */ - public PID(double kp, double ki, double kd, double kf, double tolerance) { - gains = new Gains(kp, ki, kd, kf, 0); - this.tolerance = tolerance; - } - public PID(Gains gains, double tolerance) { this.gains = gains; - this.tolerance = tolerance; } // Called when the command is initially scheduled. @@ -244,10 +184,5 @@ public class GotoLastApril extends Command { return output; } - - // // Returns true when the command should end. - // public final boolean isFinished() { - // return Math.abs(getError()) < tolerance; - // } } } diff --git a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java similarity index 83% rename from src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java rename to src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index cda04a7..b3b35a4 100644 --- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -1,12 +1,9 @@ -package frc4388.robot.commands; - -import java.time.Instant; +package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.TimesNegativeOne; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { @@ -15,15 +12,13 @@ public class DriveUntilLiDAR extends Command { private final Translation2d rightStick; private final Lidar m_lidar; private final double mindistance; - private final boolean robotRelative; public DriveUntilLiDAR( SwerveDrive swerveDrive, Translation2d leftStick, Translation2d rightStick, Lidar lidar, - double mindistance, - boolean robotRelative) { + double mindistance) { addRequirements(swerveDrive); this.swerveDrive = swerveDrive; @@ -31,7 +26,6 @@ public class DriveUntilLiDAR extends Command { this.rightStick = rightStick; this.m_lidar = lidar; this.mindistance = mindistance; - this.robotRelative = robotRelative; } @Override diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java similarity index 97% rename from src/main/java/frc4388/robot/commands/LidarAlign.java rename to src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index 8da22a3..c40ca26 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -41,7 +41,7 @@ public class LidarAlign extends Command { this.currentFinderTick = 0; this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; - this.headedRight = (GotoLastApril.tagRelativeXError < 0); + this.headedRight = (DriveToReef.tagRelativeXError < 0); this.additionalDistance = 0; this.bounces = 0; } diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/autos/neo AutoRecoding format.txt similarity index 100% rename from src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt rename to src/main/java/frc4388/robot/commands/autos/neo AutoRecoding format.txt diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/autos/neoPlaybackChooser.java similarity index 99% rename from src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java rename to src/main/java/frc4388/robot/commands/autos/neoPlaybackChooser.java index 86bc7b2..a069786 100644 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/autos/neoPlaybackChooser.java @@ -1,3 +1,4 @@ +package frc4388.robot.commands.autos; // package frc4388.robot.commands.Autos; // import java.io.File; diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java similarity index 95% rename from src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java rename to src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java index a2945c0..fea8b77 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands.Swerve; +package frc4388.robot.commands.swerve; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java similarity index 94% rename from src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java rename to src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java index 8b5afdf..f1c865e 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.Swerve; +package frc4388.robot.commands.swerve; import java.io.FileInputStream; import java.util.ArrayList; @@ -7,10 +7,10 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.VirtualController; +import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; /** @@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command { private final Supplier filenameGetter; private String filename; private int frame_index = 0; - private long startTime = 0; - private long playbackTime = 0; + // private long startTime = 0; + // private long playbackTime = 0; private boolean m_finished = false; // ! There is no better way. private boolean m_shouldfree = false; // should free memory on ending @@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command { @Override public void initialize() { - startTime = System.currentTimeMillis(); - playbackTime = 0; + // startTime = System.currentTimeMillis(); + // playbackTime = 0; frame_index = 0; m_finished = !loadAuto(); diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java similarity index 96% rename from src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java rename to src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java index 7f48a6c..1df53b6 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java @@ -1,4 +1,4 @@ -package frc4388.robot.commands.Swerve; +package frc4388.robot.commands.swerve; import java.io.FileOutputStream; import java.util.ArrayList; @@ -8,10 +8,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.DeadbandedXboxController; +import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; /** * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s diff --git a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java similarity index 97% rename from src/main/java/frc4388/robot/commands/waitElevatorRefrence.java rename to src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java index f3bd0fc..7108de5 100644 --- a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Elevator; diff --git a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java similarity index 97% rename from src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java rename to src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java index 19fe2a8..73fd893 100644 --- a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Elevator; diff --git a/src/main/java/frc4388/robot/commands/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java similarity index 96% rename from src/main/java/frc4388/robot/commands/waitFeedCoral.java rename to src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java index e1e32f6..2f66710 100644 --- a/src/main/java/frc4388/robot/commands/waitFeedCoral.java +++ b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Elevator; diff --git a/src/main/java/frc4388/robot/commands/waitSupplier.java b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java similarity index 96% rename from src/main/java/frc4388/robot/commands/waitSupplier.java rename to src/main/java/frc4388/robot/commands/wait/waitSupplier.java index 0ea5a3b..a23db69 100644 --- a/src/main/java/frc4388/robot/commands/waitSupplier.java +++ b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import java.util.function.Supplier; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java new file mode 100644 index 0000000..1d3b3ad --- /dev/null +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -0,0 +1,19 @@ +package frc4388.robot.constants; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "2025RidgeScape"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 164; + public static final String GIT_SHA = "06d525ade1118ae193383bc86066ea5563d86a1f"; + public static final String GIT_DATE = "2025-04-22 18:21:00 EDT"; + public static final String GIT_BRANCH = "advantagekit"; + public static final String BUILD_DATE = "2025-07-11 15:07:31 EDT"; + public static final long BUILD_UNIX_TIME = 1752260851324L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java new file mode 100644 index 0000000..add6b2d --- /dev/null +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -0,0 +1,234 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.constants; + +import com.ctre.phoenix6.configs.Slot0Configs; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import frc4388.utility.Trim; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; +import frc4388.utility.structs.LEDPatterns; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final String CANBUS_NAME = "rio"; + + public static final class LiDARConstants { + public static final int REEF_LIDAR_DIO_CHANNEL = 7; + public static final int REVERSE_LIDAR_DIO_CHANNEL = 4; + + public static final int HUMAN_PLAYER_STATION_DISTANCE = 40; + + public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole + public static final int LIDAR_MICROS_TO_CM = 10; + public static final int SECONDS_TO_MICROS = 1000000; + } + + public static final class AutoConstants { + // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); + public static final Gains XY_GAINS = new Gains(8,0,0.0); + // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); + // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); + // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); + // public static final Gains XY_GAINS = new Gains(3,0.3,0.0); + + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007); + // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0); + // public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5); + public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); + public static final Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0); + public static final Trim ARM_OFFSET_TRIM = new Trim("ARM Offset Trim", -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 0); + + public static final double XY_TOLERANCE = 0.07; // Meters + public static final double ROT_TOLERANCE = 5; // Degrees + + public static final double MIN_XY_PID_OUTPUT = 0.0; + public static final double MIN_ROT_PID_OUTPUT = 0.0; + + public static final double VELOCITY_THRESHHOLD = 0.01; + + // X is tangent to reef side + // Y is normal to reef side + public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field + public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1); + public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18); + + public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP; + // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5); + + public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15); + public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1); + + public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6); + public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2); + + public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE; + + // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5); + // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5); + + // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15); + // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1); + + // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5); + + // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2); + + public static final int L4_DRIVE_TIME = 250; //Milliseconds + // public static final int L3_DRIVE_TIME = 500; + public static final int L2_DRIVE_TIME = 250; //Milliseconds + public static final int ALGAE_DRIVE_TIME = 500; + public static final double STOP_VELOCITY = 0.0; + } + + public static final class VisionConstants { + public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; + public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; + + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0)); + + public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters + + // Photonvision thing + // The standard deviations of our vision estimated poses, which affect correction rate + // X, Y, Theta + // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); + } + + public static final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + // Test april tag field layout + // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + // Arrays.asList(new AprilTag[] { + // new AprilTag(1, new Pose3d( + // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + // )), + // }), 100, 100); + + } + + public static final class LEDConstants { + public static final int LED_SPARK_ID = 9; + + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + + public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; + public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; + public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; + + public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; + public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTONBOX_ID = 2; + public static final int XBOX_PROGRAMMER_ID = 3; + public static final double LEFT_AXIS_DEADBAND = 0.1; + + } + + public static final class ElevatorConstants { + public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15); + public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16); + + public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75; + public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20; + + // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + + public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND + public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND + public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND + + + public static final double GEAR_RATIO_ELEVATOR = -9.0; + //Max for elevator = 50% + + public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; + public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe + public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe + public static final double SCORING_THREE_ELEVATOR = -9.25; + public static final double DEALGAE_L2_ELEVATOR = -4; + public static final double DEALGAE_L3_ELEVATOR = -26.5; + public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position + + public static final double GEAR_RATIO_ENDEFECTOR = -100.0; + public static final double ENDEFECTOR_DRIVE_SLOW = -0.08; + //Max for endefector = 60% + public static final double L2_SCORE_ELEVATOR = -5; + public static final double L2_LEAVE_ELEVATOR = -11; + + public static final double L2_SCORE_ENDEFFECTOR = -19; + + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; + public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; + + public static final Slot0Configs ELEVATOR_PID = new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(0); + + public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs() + .withKP(1) + .withKI(0) + .withKD(0); + } + + // Logging constants + public static class SimConstants { + public static final Mode simMode = Mode.SIM; + public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; + + public static enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + + /** Replaying from a log file. */ + REPLAY + } + } +} diff --git a/src/main/java/frc4388/robot/constants/DriveConstants.java b/src/main/java/frc4388/robot/constants/DriveConstants.java new file mode 100644 index 0000000..1fa526b --- /dev/null +++ b/src/main/java/frc4388/robot/constants/DriveConstants.java @@ -0,0 +1,247 @@ +package frc4388.robot.constants; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; +import frc4388.utility.structs.Gains; + + +// No mans land +// Beware, there be dragons. +public final class DriveConstants { + public static final double MAX_ROT_SPEED = Math.PI * 2; + public static final double AUTO_MAX_ROT_SPEED = 1.5; + public static final double MIN_ROT_SPEED = 1.0; + public static double ROTATION_SPEED = MAX_ROT_SPEED; + public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; + public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final double CORRECTION_MIN = 10; + public static final double CORRECTION_MAX = 50; + + public static final double SLOW_SPEED = 0.25; + public static final double FAST_SPEED = 0.5; + public static final double TURBO_SPEED = 1.0; + + public static final double[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED}; + public static final int STARTING_GEAR = 0; + // Dimensions + public static final double WIDTH = 18.5; // TODO: validate + public static final double HEIGHT = 18.5; // TODO: validate + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; + + // Mechanics + private static final double COUPLE_RATIO = 3; //todo: find + private static final double DRIVE_RATIO = 6.12; + private static final double STEER_RATIO = (150/7); + private static final Distance WHEEL_RADIUS = Inches.of(2); + + public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate + + public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084; + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; + + // Operation + public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 + + public static final boolean DRIFT_CORRECTION_ENABLED = true; + public static final boolean INVERT_X = false; + public static final boolean INVERT_Y = true; + public static final boolean INVERT_ROTATION = false; + + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); + + private static final class ModuleSpecificConstants { //2025 + //Front Left + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); + private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; + private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Front Right + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); + private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; + private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH); + private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + + //Back Left + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); + private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_LEFT_ENCODER_INVERTED = false; + private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); + + //Back Right + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; + private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; + private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; + private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH); + private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); + } + + public static final class IDs { + public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4); + public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5); + public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11); + + public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 2); + public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 3); + public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 10); + + public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6); + public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7); + public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12); + + public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8); + public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9); + public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); + + public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); + } + + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0); + + public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(50).withKI(0).withKD(0.32) + .withKS(0).withKV(0).withKA(0); + + public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs() + .withKP(10).withKI(0).withKD(0.3) + .withKS(0).withKV(0).withKA(0); + + public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.6) + .withKS(0.1).withKV(1.91).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); + } + + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double NEUTRAL_DEADBAND = 0.04; + + // POWER! (limiting) + private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND) + ).withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE) + ).withClosedLoopRamps( + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE) + ); + private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND) + // ).withOpenLoopRamps( + // new OpenLoopRampsConfigs() + // .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE) + // ).withClosedLoopRamps( + // new ClosedLoopRampsConfigs() + // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) + ); + public static final double SLIP_CURRENT = 60; // TODO: Tune??? + } + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(IDs.DRIVE_PIGEON.id); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() // holy verbosity batman. + .withDriveMotorGearRatio(DRIVE_RATIO) + .withSteerMotorGearRatio(STEER_RATIO) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ? + .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ? + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage) + .withSlipCurrent(Configurations.SLIP_CURRENT) + .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.RemoteCANcoder) + .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS) + .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS); + + public static final SwerveModuleConstants FRONT_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, + ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants FRONT_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, + ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_LEFT = + ConstantCreator.createModuleConstants( + IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, + ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED + ); + public static final SwerveModuleConstants BACK_RIGHT = + ConstantCreator.createModuleConstants( + IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET, + ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, + ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED + ); + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + +} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index a95c0a7..9e6ca1c 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -12,12 +12,11 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.constants.DriveConstants; import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; +import frc4388.utility.compute.RobotTime; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index cbd10bb..cf45643 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,32 +4,18 @@ package frc4388.robot.subsystems; -import java.time.Instant; - -import org.opencv.ml.RTrees; - -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ElevatorConstants; -import frc4388.robot.Constants.LEDConstants; -import frc4388.robot.Constants.AutoConstants; -import frc4388.robot.subsystems.LED; -import frc4388.utility.LEDPatterns; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.TimesNegativeOne; -import frc4388.utility.Status.ReportLevel; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.ElevatorConstants; +import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.ReportLevel; public class Elevator extends Subsystem { /** Creates a new Elevator. */ @@ -37,6 +23,7 @@ public class Elevator extends Subsystem { private TalonFX endeffectorMotor; private LED led; + @SuppressWarnings("unused") private long wait = 0; private long maxWait = 1000; @@ -278,6 +265,7 @@ public class Elevator extends Subsystem { transitionState(CoordinationState.Hovering); } + @SuppressWarnings("unused") private void periodicScoring() { if (!endeffectorLimitSwitch.get()) transitionState(CoordinationState.Waiting); diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e85100f..6d13a87 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,18 +7,14 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.LEDConstants; -import frc4388.utility.LEDPatterns; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; +import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.structs.LEDPatterns; /** * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index e660301..3f3027f 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -1,16 +1,15 @@ package frc4388.robot.subsystems; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.Counter; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import frc4388.robot.Constants.LiDARConstants; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.ReportLevel; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface public class Lidar extends Subsystem { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index c1f9335..d453af9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,7 +5,8 @@ package frc4388.robot.subsystems; import java.util.Optional; -import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -14,34 +15,26 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.commands.GotoLastApril; -import frc4388.robot.commands.LidarAlign; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.TimesNegativeOne; -import frc4388.utility.Status.ReportLevel; +import frc4388.robot.constants.DriveConstants; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.ReportLevel; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.RobotConfig; public class SwerveDrive extends Subsystem { @@ -49,13 +42,14 @@ public class SwerveDrive extends Subsystem { private Vision vision; - private int gear_index = SwerveDriveConstants.STARTING_GEAR; + private int gear_index = DriveConstants.STARTING_GEAR; private boolean stopped = false; + @SuppressWarnings("unused") private boolean robotKnowsWhereItIs = false; - public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% public double lastOdomSpeed; @@ -115,6 +109,30 @@ public class SwerveDrive extends Subsystem { this // Reference to this subsystem to set requirements ); + PathPlannerLogging.setLogActivePathCallback( + (activePath) -> { + Logger.recordOutput( + "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + }); + + PathPlannerLogging.setLogTargetPoseCallback( + (targetPose) -> { + Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + }); + + + + // // Configure SysId + // sysId = + // new SysIdRoutine( + // new SysIdRoutine.Config( + // null, + // null, + // null, + // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + } public void setOdoPose(Pose2d pose) { @@ -153,7 +171,7 @@ public class SwerveDrive extends Subsystem { rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); // ! drift correction - if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX() * speedAdjust) @@ -167,9 +185,9 @@ public class SwerveDrive extends Subsystem { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD ); swerveDriveTrain.setControl(ctrl); SmartDashboard.putBoolean("drift correction", true); @@ -189,8 +207,8 @@ public class SwerveDrive extends Subsystem { // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } @@ -224,9 +242,9 @@ public class SwerveDrive extends Subsystem { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD ); swerveDriveTrain.setControl(ctrl); } @@ -239,9 +257,9 @@ public class SwerveDrive extends Subsystem { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); // ctrl.HeadingController.setPID( - // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - // SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD // ); swerveDriveTrain.setControl(ctrl); } @@ -263,7 +281,7 @@ public class SwerveDrive extends Subsystem { } public void deactivateLuigiMode() { - setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + setLimits(DriveConstants.Configurations.SLIP_CURRENT); } public boolean rotateToTarget(double angle) { @@ -342,77 +360,65 @@ public class SwerveDrive extends Subsystem { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - if (vision.isTag()) { - Pose2d pose = vision.getPose2d(); - if (!robotKnowsWhereItIs) { - robotKnowsWhereItIs = true; - Pose2d currPose = getPose2d(); - double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees(); - rotTarget += deltaAngle; - } - - vision.addVisionMeasurement(swerveDriveTrain); - // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); - //back to the ~~future~~ *past* - } + // if (vision.isTag()) {5 // if(e.isPresent()) } private void reset_index() { - gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index - 1; if (i == -1) i = 0; - setPercentOutput(SwerveDriveConstants.GEARS[i]); + setPercentOutput(DriveConstants.GEARS[i]); gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index + 1; - if (i == SwerveDriveConstants.GEARS.length) - i = SwerveDriveConstants.GEARS.length - 1; - setPercentOutput(SwerveDriveConstants.GEARS[i]); + if (i == DriveConstants.GEARS.length) + i = DriveConstants.GEARS.length - 1; + setPercentOutput(DriveConstants.GEARS[i]); gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; } public void setToSlow() { - setPercentOutput(SwerveDriveConstants.SLOW_SPEED); + setPercentOutput(DriveConstants.SLOW_SPEED); gear_index = 0; } public void setToFast() { - setPercentOutput(SwerveDriveConstants.FAST_SPEED); + setPercentOutput(DriveConstants.FAST_SPEED); gear_index = 1; } public void setToTurbo() { - setPercentOutput(SwerveDriveConstants.TURBO_SPEED); + setPercentOutput(DriveConstants.TURBO_SPEED); gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; + rotSpeedAdjust = DriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; } - private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; + private int tmp_gear_index = DriveConstants.STARTING_GEAR; public void startSlowPeriod() { tmp_gear_index = gear_index; @@ -425,7 +431,7 @@ public class SwerveDrive extends Subsystem { } public void endSlowPeriod() { - setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); + setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; } @@ -435,17 +441,10 @@ public class SwerveDrive extends Subsystem { if(curPose.isPresent() && lastPose.isPresent()){ this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - - // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees(); - - // if(rotDiff >= 180){ - // vision.subtractRotation(); - // }else if(rotDiff <= -180){ - // vision.addRotation(); - // } SmartDashboard.putNumber("Speed", lastOdomSpeed); } } + @Override @@ -484,4 +483,11 @@ public class SwerveDrive extends Subsystem { return status; } + + + // Update CTRE simulation, if used. + public void updateSim(double voltage) { + swerveDriveTrain.updateSimState(0.02, voltage); + } } + diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 4f34193..d6211a0 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -1,15 +1,11 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.time.Instant; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -18,13 +14,7 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; @@ -37,11 +27,11 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.FieldConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; +import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.Subsystem; +import frc4388.utility.status.Status.ReportLevel; public class Vision extends Subsystem { @@ -104,39 +94,26 @@ public class Vision extends Subsystem { photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; - // resetRotations(); } @Override public void periodic() { update(); field.setRobotPose(getPose2d()); - // cameras[0]. } - // public int[] rotations; - // public Instant[] lastUpdateTimes; - - // public void resetRotations(){ - // rotations = new int[cameras.length]; - // lastUpdateTimes = new Instant[cameras.length]; - // } - - private Instant lastVisionTime = null; + // private Instant lastVisionTime = null; private void update() { isTagProcessed = false; isTagDetected = false; - Instant now = Instant.now(); + // Instant now = Instant.now(); - int cams = 0; + // int cams = 0; - // double X = 0; - // double Y = 0; - // double Yaw = 0; - double latency = 0; + // double latency = 0; // Pose2d pose = null; poses.clear(); @@ -153,7 +130,7 @@ public class Vision extends Subsystem { var result = results.get(results.size()-1); - latency += result.getTimestampSeconds(); + // latency += result.getTimestampSeconds(); isTagDetected = isTagDetected | result.hasTargets(); @@ -185,21 +162,6 @@ public class Vision extends Subsystem { } - - // lastLatency = latency / cams; - - // if(isTagProcessed){ - - - // lastVisionPose = pose; - // // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); - // // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); - - // // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); - // // SmartDashboard.putNumber("Rotations", rotations); - - // lastVisionTime = now; - // } } @@ -315,11 +277,11 @@ public class Vision extends Subsystem { public Pose2d getPose2d() { if(lastPhysOdomPose != null) return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; return new Pose2d(); - // if(isTagDetected && isTagProcessed) - // // return lastVisionPose; - // else - // return lastPhysOdomPose; + } public static double getTime() { diff --git a/src/main/java/frc4388/utility/Alliance.java b/src/main/java/frc4388/utility/Alliance.java deleted file mode 100644 index 0f96914..0000000 --- a/src/main/java/frc4388/utility/Alliance.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc4388.utility; - -public class Alliance { - -} diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java deleted file mode 100644 index c2ad269..0000000 --- a/src/main/java/frc4388/utility/AprilTag.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc4388.utility; - -// This is a seperate class in case I want to encode rotation or other -// information about the tag -public class AprilTag { - public final double x, y, z; - - public AprilTag(double _x, double _y, double _z) { - x = _x; - y = _y; - z = _z; - } -} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 3ae503f..2f9f671 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -15,6 +15,8 @@ import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; +import frc4388.utility.compute.RobotTime; +import frc4388.utility.compute.RobotUnits; /** * Gyro class that allows for interchangeable use between a pigeon and a navX diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/Trim.java index 9b06575..267d09b 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/Trim.java @@ -11,6 +11,7 @@ import java.util.ArrayList; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc4388.utility.compute.DataUtils; /** * Reboot persistant Trims. @@ -27,11 +28,34 @@ public class Trim { private boolean modified = false; private double currentValue; + private boolean persistant = false; private GenericEntry trimElement = null; - + /** - * Creates a Trim with a given name, upper and lower bounds, step size and intial value + * Creates a variably Trim with a given name, upper and lower bounds, step size and intial value + * @param trimName please keep the trim name without special symbols + * @param upperBound the upper limit inclusive + * @param lowerBound the lower limit inclusive + * @param step the step size + * @param inital the inital value, will get overridden if the persistant trim exists on disk. + * @param persistnat Weather the trim is persistant or not + */ + public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) { + this.trimName = trimName; + this.upperBound = upperBound; + this.lowerBound = lowerBound; + this.step = step; + this.persistant = persistant; + currentValue = inital; + load(); + trimElement = trimTab.add(trimName, currentValue).getEntry(); + + trims.add(this); + } + + /** + * Creates a non-Trim with a given name, upper and lower bounds, step size and intial value * @param trimName please keep the trim name without special symbols * @param upperBound the upper limit inclusive * @param lowerBound the lower limit inclusive @@ -81,22 +105,25 @@ public class Trim { } public boolean load() { - // try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { - // double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - // currentValue = fileValue; - // clampModify(); - // modified = false; - // if (fileValue != currentValue) { - // System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); - // modified = true; - // } - // return true; - // } catch (Exception e) { - // // e.printStackTrace(); - // System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); - // return false; - // } - return false; + if(!persistant) + return false; + + try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) { + double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8)); + currentValue = fileValue; + clampModify(); + modified = false; + if (fileValue != currentValue) { + System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping..."); + modified = true; + } + return true; + } catch (Exception e) { + // e.printStackTrace(); + System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value..."); + return false; + } + } public void dump() { diff --git a/src/main/java/frc4388/utility/DataUtils.java b/src/main/java/frc4388/utility/compute/DataUtils.java similarity index 96% rename from src/main/java/frc4388/utility/DataUtils.java rename to src/main/java/frc4388/utility/compute/DataUtils.java index 3d998b6..3b83190 100644 --- a/src/main/java/frc4388/utility/DataUtils.java +++ b/src/main/java/frc4388/utility/compute/DataUtils.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.compute; import java.nio.ByteBuffer; diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java similarity index 93% rename from src/main/java/frc4388/utility/ReefPositionHelper.java rename to src/main/java/frc4388/utility/compute/ReefPositionHelper.java index 75c4ad0..73dc6a5 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/compute/ReefPositionHelper.java @@ -1,15 +1,11 @@ -package frc4388.utility; - -import java.util.Optional; +package frc4388.utility.compute; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc4388.robot.Constants.AutoConstants; -import frc4388.robot.Constants.FieldConstants; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.FieldConstants; public class ReefPositionHelper { public enum Side { diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/compute/RobotTime.java similarity index 98% rename from src/main/java/frc4388/utility/RobotTime.java rename to src/main/java/frc4388/utility/compute/RobotTime.java index 694f850..ebb43d8 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/compute/RobotTime.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.utility; +package frc4388.utility.compute; /** *

Keeps track of Robot times like time passed, delta time, etc diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/compute/RobotUnits.java similarity index 96% rename from src/main/java/frc4388/utility/RobotUnits.java rename to src/main/java/frc4388/utility/compute/RobotUnits.java index 9e07312..0f76d06 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/compute/RobotUnits.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.utility; +package frc4388.utility.compute; /** Aarav's good units class (better than WPILib) * @author Aarav Shah */ diff --git a/src/main/java/frc4388/utility/TimesNegativeOne.java b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java similarity index 68% rename from src/main/java/frc4388/utility/TimesNegativeOne.java rename to src/main/java/frc4388/utility/compute/TimesNegativeOne.java index bf9818f..9b7da0e 100644 --- a/src/main/java/frc4388/utility/TimesNegativeOne.java +++ b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.compute; import java.util.Optional; @@ -7,16 +7,16 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.constants.DriveConstants; // Class that holds weather the drivers sticks should be inverted public class TimesNegativeOne { - public static boolean XAxis = SwerveDriveConstants.INVERT_X; - public static boolean YAxis = SwerveDriveConstants.INVERT_Y; - public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; + public static boolean XAxis = DriveConstants.INVERT_X; + public static boolean YAxis = DriveConstants.INVERT_Y; + public static boolean RotAxis = DriveConstants.INVERT_ROTATION; public static boolean isRed = false; - public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET); private static boolean isRed() { Optional alliance = DriverStation.getAlliance(); @@ -26,10 +26,10 @@ public class TimesNegativeOne { public static void update(){ isRed = isRed(); - XAxis = SwerveDriveConstants.INVERT_X ^ isRed; - YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; - RotAxis = SwerveDriveConstants.INVERT_ROTATION; - ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + XAxis = DriveConstants.INVERT_X ^ isRed; + YAxis = DriveConstants.INVERT_Y ^ isRed; + RotAxis = DriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); SmartDashboard.putBoolean("Is red alliance", isRed); } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 4577a2e..75fd153 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,6 +1,6 @@ package frc4388.utility.controller; -import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; +import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; diff --git a/src/main/java/frc4388/utility/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java similarity index 83% rename from src/main/java/frc4388/utility/CanDevice.java rename to src/main/java/frc4388/utility/status/CanDevice.java index 1805403..ef72647 100644 --- a/src/main/java/frc4388/utility/CanDevice.java +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -1,13 +1,9 @@ -package frc4388.utility; +package frc4388.utility.status; import java.util.ArrayList; import java.util.List; -import edu.wpi.first.hal.CANData; -import edu.wpi.first.hal.can.CANJNI; -import edu.wpi.first.wpilibj.CAN; -import frc4388.utility.Status.Report; -import frc4388.utility.Status.ReportLevel; +import frc4388.utility.status.Status.ReportLevel; public class CanDevice { public static List devices = new ArrayList<>(); diff --git a/src/main/java/frc4388/utility/Status.java b/src/main/java/frc4388/utility/status/Status.java similarity index 99% rename from src/main/java/frc4388/utility/Status.java rename to src/main/java/frc4388/utility/status/Status.java index bf0de19..22e8a5f 100644 --- a/src/main/java/frc4388/utility/Status.java +++ b/src/main/java/frc4388/utility/status/Status.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.status; import java.util.ArrayList; import java.util.List; diff --git a/src/main/java/frc4388/utility/Subsystem.java b/src/main/java/frc4388/utility/status/Subsystem.java similarity index 95% rename from src/main/java/frc4388/utility/Subsystem.java rename to src/main/java/frc4388/utility/status/Subsystem.java index 961900d..15bb147 100644 --- a/src/main/java/frc4388/utility/Subsystem.java +++ b/src/main/java/frc4388/utility/status/Subsystem.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.status; import java.util.ArrayList; import java.util.List; diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/structs/Gains.java similarity index 98% rename from src/main/java/frc4388/utility/Gains.java rename to src/main/java/frc4388/utility/structs/Gains.java index 7a3a026..0d5b674 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/structs/Gains.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.utility; +package frc4388.utility.structs; /** Add your docs here. */ public class Gains { diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/structs/LEDPatterns.java similarity index 98% rename from src/main/java/frc4388/utility/LEDPatterns.java rename to src/main/java/frc4388/utility/structs/LEDPatterns.java index 6df032c..585dc43 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/structs/LEDPatterns.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.structs; /** * Add your docs here. diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/structs/UtilityStructs.java similarity index 95% rename from src/main/java/frc4388/utility/UtilityStructs.java rename to src/main/java/frc4388/utility/structs/UtilityStructs.java index 935dbbe..2307f2f 100644 --- a/src/main/java/frc4388/utility/UtilityStructs.java +++ b/src/main/java/frc4388/utility/structs/UtilityStructs.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.structs; public class UtilityStructs { public static class TimedOutput { diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.4.json rename to vendordeps/PathplannerLib-2025.2.7.json index 24add57..d3f84e5 100644 --- a/vendordeps/PathplannerLib-2025.2.4.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.4.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 75% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-frc2025-latest.json index 473f6a8..6f40c84 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +379,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index db43d6d..2d7b1d8 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-8", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-8", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-8" + "version": "v2025.3.1" } ] } \ No newline at end of file From c183c08a3d92ff8561bef96dad52b8dd64d94f14 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 11 Jul 2025 17:58:22 -0600 Subject: [PATCH 02/10] Slightly overhaul status system --- src/main/java/frc4388/robot/Robot.java | 99 +--------- src/main/java/frc4388/robot/RobotMap.java | 33 +++- .../robot/constants/BuildConstants.java | 10 +- .../robot/constants/DriveConstants.java | 1 - .../frc4388/robot/subsystems/DiffDrive.java | 12 +- .../frc4388/robot/subsystems/Elevator.java | 12 +- .../java/frc4388/robot/subsystems/LED.java | 15 +- .../java/frc4388/robot/subsystems/Lidar.java | 19 +- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +- .../java/frc4388/robot/subsystems/Vision.java | 39 +--- .../frc4388/utility/status/FaultCANCoder.java | 82 +++++++++ .../utility/status/FaultPhotonCamera.java | 38 ++++ .../frc4388/utility/status/FaultPidgeon2.java | 174 ++++++++++++++++++ .../frc4388/utility/status/FaultReporter.java | 129 +++++++++++++ .../frc4388/utility/status/FaultTalonFX.java | 160 ++++++++++++++++ .../frc4388/utility/status/QueryUtils.java | 13 ++ .../frc4388/utility/status/Queryable.java | 10 + .../java/frc4388/utility/status/Status.java | 4 + .../frc4388/utility/status/Subsystem.java | 25 --- 19 files changed, 698 insertions(+), 194 deletions(-) create mode 100644 src/main/java/frc4388/utility/status/FaultCANCoder.java create mode 100644 src/main/java/frc4388/utility/status/FaultPhotonCamera.java create mode 100644 src/main/java/frc4388/utility/status/FaultPidgeon2.java create mode 100644 src/main/java/frc4388/utility/status/FaultReporter.java create mode 100644 src/main/java/frc4388/utility/status/FaultTalonFX.java create mode 100644 src/main/java/frc4388/utility/status/QueryUtils.java create mode 100644 src/main/java/frc4388/utility/status/Queryable.java delete mode 100644 src/main/java/frc4388/utility/status/Subsystem.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 6b948bf..bb38c33 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,10 +7,6 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.Base64; -import java.util.List; - // Advantagekit import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -19,23 +15,16 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.CANBus.CANBusStatus; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; -import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; import frc4388.utility.DeferredBlockMulti; import frc4388.utility.Trim; import frc4388.utility.compute.RobotTime; -import frc4388.utility.status.CanDevice; -import frc4388.utility.status.Status; -import frc4388.utility.status.Subsystem; -import frc4388.utility.status.Status.Report; -import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.status.FaultReporter; + //import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the @@ -63,23 +52,7 @@ public class Robot extends LoggedRobot { // Create a shuffleboard update thread, that will periodically update the values on shuffleboard - new Thread() { - public void run() { - try{ - while(!this.isInterrupted() && this.isAlive()){ - Thread.sleep(500); - for(int i=0;i errors = new ArrayList<>(); - - // Subsystems header - System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY"))); - - for(int i=0;i< Subsystem.subsystems.size();i++){ - - Subsystem subsystem = Subsystem.subsystems.get(i); - System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":"); - Status status = subsystem.diagnosticStatus(); - - for(int a=0;a 0) { - // Errors header - System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY"))); - for(int i=0;i swerveDriveTrain; private Vision vision; @@ -65,7 +66,7 @@ public class SwerveDrive extends Subsystem { public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain // swerveDriveTrain) { - super(); + FaultReporter.register(this); this.swerveDriveTrain = swerveDriveTrain; this.vision = vision; @@ -448,12 +449,12 @@ public class SwerveDrive extends Subsystem { @Override - public String getSubsystemName() { + public String getName() { return "Swerve Drive Controller"; } ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .getLayout(getName(), BuiltInLayouts.kList) .withSize(2, 2); GenericEntry sbGyro = subsystemLayout @@ -478,8 +479,8 @@ public class SwerveDrive extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); - status.addReport(ReportLevel.INFO, - "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + // status.addReport(ReportLevel.INFO, + // "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); return status; } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index d6211a0..5a85772 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -27,13 +27,14 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; import frc4388.utility.status.Status; -import frc4388.utility.status.Subsystem; -import frc4388.utility.status.Status.ReportLevel; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; -public class Vision extends Subsystem { +public class Vision extends SubsystemBase implements Queryable { // private PhotonCamera leftCamera; // private PhotonCamera rightCamera; @@ -59,7 +60,7 @@ public class Vision extends Subsystem { private Field2d field = new Field2d(); ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .getLayout(getName(), BuiltInLayouts.kList) .withSize(2, 2); GenericEntry sbTagDetected = subsystemLayout @@ -72,17 +73,8 @@ public class Vision extends Subsystem { .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - GenericEntry sbLeftCamConnected = subsystemLayout - .add("Left Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbRightCamConnected = subsystemLayout - .add("Right Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ + FaultReporter.register(this); SmartDashboard.putData(field); this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; @@ -307,7 +299,7 @@ public class Vision extends Subsystem { @Override - public String getSubsystemName() { + public String getName() { return "Vision"; } @@ -321,27 +313,12 @@ public class Vision extends Subsystem { public void queryStatus() { sbTagDetected.setBoolean(isTagDetected); sbTagProcessed.setBoolean(isTagProcessed); - sbLeftCamConnected.setBoolean(cameras[0].isConnected()); - sbRightCamConnected.setBoolean(cameras[1].isConnected()); // field.setRobotPose(getPose2d()); } @Override public Status diagnosticStatus() { - Status status = new Status(); - - if(cameras[0].isConnected()) - status.addReport(ReportLevel.INFO, "Left Camera Connected"); - else - status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED"); - - if(cameras[1].isConnected()) - status.addReport(ReportLevel.INFO, "Right Camera Connected"); - else - status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED"); - - - return status; + return new Status(); } } diff --git a/src/main/java/frc4388/utility/status/FaultCANCoder.java b/src/main/java/frc4388/utility/status/FaultCANCoder.java new file mode 100644 index 0000000..ad11ad6 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultCANCoder.java @@ -0,0 +1,82 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.CANcoder; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultCANCoder implements Queryable { + private String name; + private CANcoder cancoder; + + public static void addDevice(CANcoder cancoder, String name) { + FaultCANCoder p = new FaultCANCoder(); + + p.name = name; + p.cancoder = cancoder; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() { + // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage()); + boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + // faults + if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Bad magnet"); + } + if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet"); + } + if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout"); + } + if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java new file mode 100644 index 0000000..0b35cb5 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java @@ -0,0 +1,38 @@ +package frc4388.utility.status; + +import org.photonvision.PhotonCamera; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPhotonCamera implements Queryable { + private String name; + private PhotonCamera cam; + + public static void addDevice(PhotonCamera cam, String name) { + FaultPhotonCamera p = new FaultPhotonCamera(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(!cam.isConnected()) + s.addReport(ReportLevel.ERROR, "Not Connected!"); + + return s; + } +} + diff --git a/src/main/java/frc4388/utility/status/FaultPidgeon2.java b/src/main/java/frc4388/utility/status/FaultPidgeon2.java new file mode 100644 index 0000000..a6f0b81 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPidgeon2.java @@ -0,0 +1,174 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.Pigeon2; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPidgeon2 implements Queryable { + private String name; + private Pigeon2 pigeon2; + + public static void addDevice(Pigeon2 pigeon2, String name) { + FaultPidgeon2 p = new FaultPidgeon2(); + + p.name = name; + p.pigeon2 = pigeon2; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public void queryStatus() { + // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + + + boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage()); + boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + + s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage()); + + s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch()); + s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw()); + s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll()); + + s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX()); + s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY()); + s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ()); + + s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX()); + s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY()); + s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ()); + + + // faults + if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while in motion"); + } + if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Accelerometer fault detected"); + } + if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro fault detected"); + } + if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Magnetometer fault detected"); + } + if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack data acquisition slower than expected"); + } + if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack loop time was slower than expected"); + } + if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Accelerometer values are saturated"); + } + if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro values are saturated"); + } + if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Magnetometer values are saturated"); + } + if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while in motion"); + } + if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Accelerometer fault detected"); + } + if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected"); + } + if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Magnetometer fault detected"); + } + if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack data acquisition slower than expected")); + } + if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack loop time was slower than expected")); + } + if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Accelerometer values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Gyro values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Magnetometer values are saturated"); + } + if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Device supply voltage near brownout"); + } + if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java new file mode 100644 index 0000000..853c727 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -0,0 +1,129 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; + +import frc4388.robot.constants.Constants; +import frc4388.utility.status.Status.Report; +import frc4388.utility.status.Status.ReportLevel; + +public class FaultReporter { + + private static final String REPORTS_HEADER = + "▄▖ ▗ \n" + // + "▙▘█▌▛▌▛▌▛▘▜▘▛▘\n" + // + "▌▌▙▖▙▌▙▌▌ ▐▖▄▌\n" + // + " ▌ "; + + private static final String CAN_HEADER = + "▄▖▄▖▖ ▖ \n" + // + "▌ ▌▌▛▖▌ \n" + // + "▙▖▛▌▌▝▌(t)\n" + // + " "; + + private static final String ERROR_HEADER = + "▄▖▄▖▄▖▄▖▄▖▄▖\n" + // + "▙▖▙▘▙▘▌▌▙▘▚ \n" + // + "▙▖▌▌▌▌▙▌▌▌▄▌\n" + // + " "; + + private static List queryables = new ArrayList<>(); + + public static void startThread() { + new Thread() { + public void run() { + try{ + while(!this.isInterrupted() && this.isAlive()){ + Thread.sleep(500); + for(int i=0;i errors = new ArrayList<>(); + + // Subsystems header + System.out.println(REPORTS_HEADER); + + for(int i=0;i< queryables.size();i++){ + + Queryable q = queryables.get(i); + System.out.println("** Subsystem diagnostic report for " + q.getName() + ":"); + Status status = q.diagnosticStatus(); + + for(int a=0;a 0) { + // Errors header + System.out.println(ERROR_HEADER); + for(int i=0;i subsystems = new ArrayList<>(); - - public Subsystem(){ - subsystems.add(this); - } - - public void Log(String str) { - System.out.println(getSubsystemName() + " - " + str); - } - - // Get name of subsystem, for use in log. - public abstract String getSubsystemName(); - // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard - public abstract void queryStatus(); - // Proactivly search for any errors in each subsystem - public abstract Status diagnosticStatus(); -} From 99bb538bcbcec9d50b6c139532ce1c8129d9352b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 13 Jul 2025 19:29:30 -0600 Subject: [PATCH 03/10] Remove RobotGyro --- src/main/java/frc4388/robot/Robot.java | 4 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/DiffDrive.java | 16 +- src/main/java/frc4388/utility/RobotGyro.java | 271 ------------------ 4 files changed, 15 insertions(+), 286 deletions(-) delete mode 100644 src/main/java/frc4388/utility/RobotGyro.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index bb38c33..57eee61 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -45,6 +45,8 @@ public class Robot extends LoggedRobot { */ @Override public void robotInit() { + // Start logging with AdvantageKit + startLogging(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. @@ -221,7 +223,7 @@ public class Robot extends LoggedRobot { // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); switch (BuildConstants.DIRTY) { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 832928d..581eeca 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 165; - public static final String GIT_SHA = "bf4da44338d8e91c50e34c3ac68dc12f9d335b08"; - public static final String GIT_DATE = "2025-07-11 14:07:53 MDT"; + public static final int GIT_REVISION = 166; + public static final String GIT_SHA = "c183c08a3d92ff8561bef96dad52b8dd64d94f14"; + public static final String GIT_DATE = "2025-07-11 17:58:22 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-11 17:46:28 MDT"; - public static final long BUILD_UNIX_TIME = 1752277588371L; + public static final String BUILD_DATE = "2025-07-13 19:23:17 MDT"; + public static final long BUILD_UNIX_TIME = 1752456197462L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 9600c18..d6dfebe 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -8,13 +8,13 @@ package frc4388.robot.subsystems; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.DriveConstants; -import frc4388.utility.RobotGyro; +// import frc4388.utility.RobotGyro; import frc4388.utility.compute.RobotTime; import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; @@ -34,13 +34,13 @@ public class DiffDrive extends SubsystemBase implements Queryable { private TalonFX m_leftBackMotor; private TalonFX m_rightBackMotor; private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; + private Pigeon2 m_gyro; /** * Add your docs here. */ public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { + TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) { FaultReporter.register(this); @@ -56,8 +56,6 @@ public class DiffDrive extends SubsystemBase implements Queryable { @Override public void periodic() { - m_gyro.updatePigeonDeltas(); - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { updateSmartDashboard(); } @@ -84,9 +82,9 @@ public class DiffDrive extends SubsystemBase implements Queryable { private void updateSmartDashboard() { // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); + // SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); + // SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); + // SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); //SmartDashboard.putData(m_gyro); } diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index 2f9f671..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,271 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -// import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; - -// import edu.wpi.first.wpilibj.GyroBase; -// import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import frc4388.utility.compute.RobotTime; -import frc4388.utility.compute.RobotUnits; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private Pigeon2 m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; //true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - private double pitchZero = 0; - private double rollZero = 0; - - /** - * Creates a Gyro based on a pigeon - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(Pigeon2 gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro){ - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Resets yaw, pitch, and roll. - */ - public void resetZeroValues() { - if (!m_isGyroAPigeon) return; - - // pitchZero = m_pigeon.getPitch(); - // rollZero = m_pigeon.getRoll(); - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

NavX: - *

Calibrate the gyro by running for a number of samples and computing the center value. Then use - * the center value as the Accumulator center value for subsequent measurements. It's important to - * make sure that the robot is not moving while the centering calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - * - *

Pigeon: - *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to - * make sure that the robot is not moving while the tempurature calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - */ - public void calibrate() { - return; - // if (m_isGyroAPigeon) { - // m_pigeon.calibrate(); - // } else { - // m_navX.calibrate(); - // } - } - - public void reset() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(0); - } else { - m_navX.reset(); - } - - } - - public void reset(double val) { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(val); - } else { - m_navX.reset(); - } - - } - - public void resetFlip() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(180); - } else { - m_navX.reset(); - } - - } - - public void resetNinety() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(90); - } else { - m_navX.reset(); - } - - } - - public void resetTwoSeventy() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(270); - } else { - m_navX.reset(); - } - - } - - public void resetRightSideBlue() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(60); - } else { - m_navX.reset(); - } - - } - - public void resetAmpSide() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(-60); - } else { - m_navX.reset(); - } - - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - //m_pigeon.getAngle(); // This appeared to not do anything? - var rotation = m_pigeon.getRotation3d(); - - return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; - } - - public Rotation2d getRotation2d() { - return m_pigeon.getRotation2d(); - } - - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[2]; - } else { - return m_navX.getAngle(); - } - } - - public double getYaw() { - return this.getAngle(); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public Pigeon2 getPigeon(){ - return m_pigeon; - } - - public AHRS getNavX(){ - return m_navX; - } - - public void close() throws Exception { - - } -} From 9fd98bce241ee2aef0855f3ce8bb0b37aee723c2 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 13 Jul 2025 19:41:05 -0600 Subject: [PATCH 04/10] Merge DeferredBlockMulti --- src/main/java/frc4388/robot/Robot.java | 2 -- .../java/frc4388/robot/RobotContainer.java | 11 +++---- .../frc4388/robot/subsystems/DiffDrive.java | 4 +-- .../java/frc4388/utility/DeferredBlock.java | 30 +++++++++++++++---- .../frc4388/utility/DeferredBlockMulti.java | 18 ----------- 5 files changed, 32 insertions(+), 33 deletions(-) delete mode 100644 src/main/java/frc4388/utility/DeferredBlockMulti.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 57eee61..fa604d7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; -import frc4388.utility.DeferredBlockMulti; import frc4388.utility.Trim; import frc4388.utility.compute.RobotTime; import frc4388.utility.status.FaultReporter; @@ -95,7 +94,6 @@ public class Robot extends LoggedRobot { @Override public void disabledExit() { DeferredBlock.execute(); - DeferredBlockMulti.execute(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ae0ae64..bb500ab 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -60,7 +60,6 @@ import frc4388.robot.subsystems.SwerveDrive; // Utilites import frc4388.utility.DeferredBlock; -import frc4388.utility.DeferredBlockMulti; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.compute.ReefPositionHelper.Side; @@ -590,12 +589,14 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> { // Called on first robot enable + + DeferredBlock.addBlock(() -> { // Called on first robot enable m_robotSwerveDrive.resetGyro(); - }); - new DeferredBlockMulti(() -> { // Called on every robot enable + }, false); + DeferredBlock.addBlock(() -> { // Called on every robot enable TimesNegativeOne.update(); - }); + }, true); + DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index d6dfebe..d70356d 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -34,7 +34,7 @@ public class DiffDrive extends SubsystemBase implements Queryable { private TalonFX m_leftBackMotor; private TalonFX m_rightBackMotor; private DifferentialDrive m_driveTrain; - private Pigeon2 m_gyro; + // private Pigeon2 m_gyro; /** * Add your docs here. @@ -51,7 +51,7 @@ public class DiffDrive extends SubsystemBase implements Queryable { m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); m_driveTrain = driveTrain; - m_gyro = gyro; + // m_gyro = gyro; } @Override diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java index 20d3c57..c5a558e 100644 --- a/src/main/java/frc4388/utility/DeferredBlock.java +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -2,22 +2,40 @@ package frc4388.utility; import java.util.ArrayList; +// Class for running code snippets whenever the robot is enabled. public class DeferredBlock { - private static ArrayList m_blocks = new ArrayList<>(); + private static ArrayList m_blocks_norerun = new ArrayList<>(); + private static ArrayList m_blocks_rerun = new ArrayList<>(); private static boolean m_hasRun = false; - public DeferredBlock(Runnable block) { - m_blocks.add(block); + public static void addBlock(Runnable block) { + addBlock(block, false); + } + + + public static void addBlock(Runnable block, boolean rerun) { + if(rerun) { + m_blocks_rerun.add(block); + } else { + m_blocks_norerun.add(block); + } } public static void execute() { - if (m_hasRun) return; - for (Runnable block : m_blocks) { + // Run blocks that run multiple times. + for (Runnable block : m_blocks_rerun) { block.run(); } - m_blocks.clear(); // for garbage collection + // Run blocks that only run once + if (m_hasRun) return; + + for (Runnable block : m_blocks_norerun) { + block.run(); + } + + m_blocks_norerun.clear(); // for garbage collection m_hasRun = true; } } diff --git a/src/main/java/frc4388/utility/DeferredBlockMulti.java b/src/main/java/frc4388/utility/DeferredBlockMulti.java deleted file mode 100644 index 9e8b284..0000000 --- a/src/main/java/frc4388/utility/DeferredBlockMulti.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc4388.utility; - -import java.util.ArrayList; - -public class DeferredBlockMulti { - private static ArrayList m_blocks = new ArrayList<>(); - - public DeferredBlockMulti(Runnable block) { - m_blocks.add(block); - } - - public static void execute() { - - for (Runnable block : m_blocks) { - block.run(); - } - } -} From aaef829ad2830d39cc9f9e05e61e973658d7784d Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 15 Jul 2025 09:33:40 -0600 Subject: [PATCH 05/10] Work on implementing adv --- simgui-ds.json | 5 +++ src/main/java/frc4388/robot/Robot.java | 19 ++++++----- .../java/frc4388/robot/RobotContainer.java | 14 ++++++-- .../robot/constants/BuildConstants.java | 10 +++--- .../frc4388/robot/constants/Constants.java | 2 +- .../frc4388/robot/subsystems/DiffDrive.java | 8 ++--- .../frc4388/robot/subsystems/Elevator.java | 4 +-- .../java/frc4388/robot/subsystems/LED.java | 8 ++--- .../java/frc4388/robot/subsystems/Lidar.java | 13 ++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++---- .../java/frc4388/robot/subsystems/Vision.java | 12 +++---- .../frc4388/utility/{ => compute}/Trim.java | 3 +- .../controller/DeadbandedXboxController.java | 1 + .../frc4388/utility/status/FaultCANCoder.java | 8 +---- .../utility/status/FaultPhotonCamera.java | 3 -- .../frc4388/utility/status/FaultPidgeon2.java | 8 +---- .../frc4388/utility/status/FaultReporter.java | 34 +++++++++---------- .../frc4388/utility/status/FaultTalonFX.java | 6 ---- .../frc4388/utility/status/Queryable.java | 2 -- 19 files changed, 83 insertions(+), 89 deletions(-) rename src/main/java/frc4388/utility/{ => compute}/Trim.java (98%) diff --git a/simgui-ds.json b/simgui-ds.json index 14c4c68..47517dc 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index fa604d7..0dfb72d 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,13 +15,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; -import frc4388.utility.Trim; import frc4388.utility.compute.RobotTime; +import frc4388.utility.compute.Trim; import frc4388.utility.status.FaultReporter; //import frc4388.robot.subsystems.LED; @@ -52,8 +53,8 @@ public class Robot extends LoggedRobot { m_robotContainer = new RobotContainer(); - // Create a shuffleboard update thread, that will periodically update the values on shuffleboard - FaultReporter.startThread(); + // // Create a shuffleboard update thread, that will periodically update the values on shuffleboard + // FaultReporter.startThread(); } /** @@ -205,13 +206,13 @@ public class Robot extends LoggedRobot { // } - // @Override - // public void simulationPeriodic() { - // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); - // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); + @Override + public void simulationPeriodic() { + m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); + // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); - // // m_robotContainer.m_robotSwerveDrive. - // } + // m_robotContainer.m_robotSwerveDrive. + } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bb500ab..07623b1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; @@ -67,7 +68,7 @@ import frc4388.utility.compute.ReefPositionHelper.Side; * This class is where the bulk of the robot should be declared. Since * Command-based is a "declarative" paradigm, very little robot logic should * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, + * scheduler calls). Instead, the structure of the robot (2including subsystems, * commands, and button mappings) should be declared here. */ public class RobotContainer { @@ -892,8 +893,15 @@ public class RobotContainer { public void makeAutoChooser() { autoChooser = new SendableChooser(); - File dir = new File("/home/lvuser/deploy/pathplanner/autos/"); - // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); + File dir; + + if(RobotBase.isReal()) { + dir = new File("/home/lvuser/deploy/pathplanner/autos/"); + } else { + // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); + dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos"); + } + String[] autos = dir.list(); if(autos == null) return; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 581eeca..10968a7 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 166; - public static final String GIT_SHA = "c183c08a3d92ff8561bef96dad52b8dd64d94f14"; - public static final String GIT_DATE = "2025-07-11 17:58:22 MDT"; + public static final int GIT_REVISION = 168; + public static final String GIT_SHA = "9fd98bce241ee2aef0855f3ce8bb0b37aee723c2"; + public static final String GIT_DATE = "2025-07-13 19:41:05 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-13 19:23:17 MDT"; - public static final long BUILD_UNIX_TIME = 1752456197462L; + public static final String BUILD_DATE = "2025-07-14 01:44:15 MDT"; + public static final long BUILD_UNIX_TIME = 1752479055707L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index add6b2d..6e8733d 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -20,7 +20,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; -import frc4388.utility.Trim; +import frc4388.utility.compute.Trim; import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; import frc4388.utility.structs.LEDPatterns; diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index d70356d..df9f29e 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -94,10 +94,10 @@ public class DiffDrive extends SubsystemBase implements Queryable { return "Diff Drive"; } - @Override - public void queryStatus() { - // TODO: Add Stuff - } + // @Override + // public void queryStatus() { + // // TODO: Add Stuff + // } @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 68a1787..3157bae 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -378,8 +378,8 @@ public class Elevator extends SubsystemBase implements Queryable { return "Elevator"; } - @Override - public void queryStatus() {} + // @Override + // public void queryStatus() {} @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 6d118cc..8f71195 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -53,10 +53,10 @@ public class LED extends SubsystemBase implements Queryable { return "LEDs"; } - @Override - public void queryStatus() { - SmartDashboard.putString("LED status", mode.name()); - } + // @Override + // public void queryStatus() { + // SmartDashboard.putString("LED status", mode.name()); + // } @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 62f1a7e..d359a7e 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -1,5 +1,7 @@ package frc4388.robot.subsystems; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Counter; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; @@ -53,6 +55,7 @@ public class Lidar extends SubsystemBase implements Queryable { distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; } + @AutoLogOutput public double getDistance(){ return distance; } @@ -71,11 +74,11 @@ public class Lidar extends SubsystemBase implements Queryable { return "Lidar " + name; } - @Override - public void queryStatus() { - sbDistance.setDouble(distance); - sbWithinDistance.setBoolean(withinDistance()); - } + // @Override + // public void queryStatus() { + // sbDistance.setDouble(distance); + // sbWithinDistance.setBoolean(withinDistance()); + // } @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 3e21681..4a11b1b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -467,13 +467,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withWidget(BuiltInWidgets.kNumberBar) .getEntry(); - @Override - public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); + // @Override + // public void queryStatus() { + // sbGyro.setDouble(getGyroAngle()); + // sbShiftState.setDouble(this.speedAdjust); - // TODO: Add more status things - } + // // TODO: Add more status things + // } @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 5a85772..b11c501 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -309,12 +309,12 @@ public class Vision extends SubsystemBase implements Queryable { // .getEntry(); - @Override - public void queryStatus() { - sbTagDetected.setBoolean(isTagDetected); - sbTagProcessed.setBoolean(isTagProcessed); - // field.setRobotPose(getPose2d()); - } + // @Override + // public void queryStatus() { + // sbTagDetected.setBoolean(isTagDetected); + // sbTagProcessed.setBoolean(isTagProcessed); + // // field.setRobotPose(getPose2d()); + // } @Override public Status diagnosticStatus() { diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/compute/Trim.java similarity index 98% rename from src/main/java/frc4388/utility/Trim.java rename to src/main/java/frc4388/utility/compute/Trim.java index 267d09b..429d526 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/compute/Trim.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.utility; +package frc4388.utility.compute; import java.io.FileInputStream; import java.io.FileOutputStream; @@ -11,7 +11,6 @@ import java.util.ArrayList; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import frc4388.utility.compute.DataUtils; /** * Reboot persistant Trims. diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 75fd153..ae4202b 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,3 +1,4 @@ + package frc4388.utility.controller; import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND; diff --git a/src/main/java/frc4388/utility/status/FaultCANCoder.java b/src/main/java/frc4388/utility/status/FaultCANCoder.java index ad11ad6..a1631fb 100644 --- a/src/main/java/frc4388/utility/status/FaultCANCoder.java +++ b/src/main/java/frc4388/utility/status/FaultCANCoder.java @@ -22,13 +22,7 @@ public class FaultCANCoder implements Queryable { public String getName() { return name; } - - @Override - public void queryStatus() { - // TODO Auto-generated method stub - // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); - } - + @Override public Status diagnosticStatus() { Status s = new Status(); diff --git a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java index 0b35cb5..5ea8b70 100644 --- a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java +++ b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java @@ -22,9 +22,6 @@ public class FaultPhotonCamera implements Queryable { return name; } - @Override - public void queryStatus() {} - @Override public Status diagnosticStatus() { Status s = new Status(); diff --git a/src/main/java/frc4388/utility/status/FaultPidgeon2.java b/src/main/java/frc4388/utility/status/FaultPidgeon2.java index a6f0b81..1e0bb4f 100644 --- a/src/main/java/frc4388/utility/status/FaultPidgeon2.java +++ b/src/main/java/frc4388/utility/status/FaultPidgeon2.java @@ -22,13 +22,7 @@ public class FaultPidgeon2 implements Queryable { public String getName() { return name; } - - @Override - public void queryStatus() { - // TODO Auto-generated method stub - // throw new UnsupportedOperationException("Unimplemented method 'queryStatus'"); - } - + @Override public Status diagnosticStatus() { Status s = new Status(); diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index 853c727..6a3831a 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -32,25 +32,25 @@ public class FaultReporter { private static List queryables = new ArrayList<>(); - public static void startThread() { - new Thread() { - public void run() { - try{ - while(!this.isInterrupted() && this.isAlive()){ - Thread.sleep(500); - for(int i=0;i Date: Tue, 15 Jul 2025 09:24:11 -0700 Subject: [PATCH 06/10] Make compile on robot --- .../robot/constants/BuildConstants.java | 10 +++---- .../java/frc4388/robot/subsystems/LED.java | 7 +++++ .../java/frc4388/robot/subsystems/Lidar.java | 22 +-------------- .../frc4388/utility/status/FaultReporter.java | 28 +++++++++++-------- 4 files changed, 29 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 10968a7..9a50e63 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 168; - public static final String GIT_SHA = "9fd98bce241ee2aef0855f3ce8bb0b37aee723c2"; - public static final String GIT_DATE = "2025-07-13 19:41:05 MDT"; + public static final int GIT_REVISION = 169; + public static final String GIT_SHA = "aaef829ad2830d39cc9f9e05e61e973658d7784d"; + public static final String GIT_DATE = "2025-07-15 09:33:40 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-14 01:44:15 MDT"; - public static final long BUILD_UNIX_TIME = 1752479055707L; + public static final String BUILD_DATE = "2025-07-15 09:49:13 MDT"; + public static final long BUILD_UNIX_TIME = 1752594553460L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 8f71195..fceffe4 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -48,6 +50,11 @@ public class LED extends SubsystemBase implements Queryable { LEDController.set(mode.getValue()); } + @AutoLogOutput + public String state() { + return mode.getClass().toString(); + } + @Override public String getName() { return "LEDs"; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index d359a7e..6981d7a 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -31,20 +31,6 @@ public class Lidar extends SubsystemBase implements Queryable { LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement LidarPWM.reset(); - - subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getName(), BuiltInLayouts.kList) - .withSize(2, 2); - - sbDistance = subsystemLayout - .add("Distance", 0) - .withWidget(BuiltInWidgets.kGraph) - .getEntry(); - - sbWithinDistance = subsystemLayout - . add("Within Distance", 0) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); } @Override @@ -55,7 +41,7 @@ public class Lidar extends SubsystemBase implements Queryable { distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; } - @AutoLogOutput + @AutoLogOutput(key = "Lidar/{name}") public double getDistance(){ return distance; } @@ -74,12 +60,6 @@ public class Lidar extends SubsystemBase implements Queryable { return "Lidar " + name; } - // @Override - // public void queryStatus() { - // sbDistance.setDouble(distance); - // sbWithinDistance.setBoolean(withinDistance()); - // } - @Override public Status diagnosticStatus() { Status s = new Status(); diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index 6a3831a..afd4dd4 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -13,22 +13,26 @@ import frc4388.utility.status.Status.ReportLevel; public class FaultReporter { private static final String REPORTS_HEADER = - "▄▖ ▗ \n" + // - "▙▘█▌▛▌▛▌▛▘▜▘▛▘\n" + // - "▌▌▙▖▙▌▙▌▌ ▐▖▄▌\n" + // - " ▌ "; + "###############\n" + // + "#.............#\n" + // + "#...Reports...#\n" + // + "#.............#\n" + // + "###############\n"; + private static final String CAN_HEADER = - "▄▖▄▖▖ ▖ \n" + // - "▌ ▌▌▛▖▌ \n" + // - "▙▖▛▌▌▝▌(t)\n" + // - " "; + "###############\n" + // + "#.............#\n" + // + "#....CAN(t)...#\n" + // + "#.............#\n" + // + "###############\n"; private static final String ERROR_HEADER = - "▄▖▄▖▄▖▄▖▄▖▄▖\n" + // - "▙▖▙▘▙▘▌▌▙▘▚ \n" + // - "▙▖▌▌▌▌▙▌▌▌▄▌\n" + // - " "; + "###############\n" + // + "#.............#\n" + // + "#....ERRORS...#\n" + // + "#.............#\n" + // + "###############\n"; private static List queryables = new ArrayList<>(); From 3130f647c83cc82b45a5299e19108f9eec45e6f6 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 15 Jul 2025 11:07:01 -0600 Subject: [PATCH 07/10] Transfer code --- src/main/java/frc4388/robot/Robot.java | 9 +- .../java/frc4388/robot/RobotContainer.java | 11 +- .../robot/commands/alignment/DriveToReef.java | 2 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/Elevator.java | 33 ++-- .../java/frc4388/robot/subsystems/LED.java | 5 - .../java/frc4388/robot/subsystems/Lidar.java | 4 - .../frc4388/robot/subsystems/SwerveDrive.java | 165 ++++++++---------- .../java/frc4388/robot/subsystems/Vision.java | 89 +++------- .../java/frc4388/utility/status/Alerts.java | 14 ++ 10 files changed, 150 insertions(+), 192 deletions(-) create mode 100644 src/main/java/frc4388/utility/status/Alerts.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0dfb72d..610f140 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,7 +15,9 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; @@ -57,6 +59,8 @@ public class Robot extends LoggedRobot { // FaultReporter.startThread(); } + + /** * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, @@ -77,7 +81,6 @@ public class Robot extends LoggedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } - /** * This function is called once each time the robot enters Disabled mode. * You can use it to reset any subsystem information you want to clear when @@ -125,6 +128,7 @@ public class Robot extends LoggedRobot { public void autonomousPeriodic() { } + @Override public void teleopInit() { m_robotContainer.stop(); @@ -137,10 +141,7 @@ public class Robot extends LoggedRobot { CommandScheduler.getInstance().cancel(m_autonomousCommand); m_autonomousCommand.cancel(); m_autonomousCommand.end(true); - System.out.println("NOT Null!!"); - } else { - System.out.println("Null!!"); } m_robotTime.startMatchTime(); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 07623b1..3608ad8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,11 +51,12 @@ import frc4388.robot.constants.Constants.OIConstants; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; + +import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; -import frc4388.robot.subsystems.Elevator; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; @@ -560,7 +561,7 @@ public class RobotContainer { NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); - NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped())); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, @@ -694,8 +695,8 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) - .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;})); new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) @@ -890,6 +891,7 @@ public class RobotContainer { // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } + public boolean autoChooserUpdated = false; public void makeAutoChooser() { autoChooser = new SendableChooser(); @@ -913,6 +915,7 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { + autoChooserUpdated = true; if (filename.equals("Taxi")) { autoCommand = new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index ac59724..11a1f82 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -131,7 +131,7 @@ public class DriveToReef extends Command { (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) ); // System.out.println(finished); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9a50e63..ae418b7 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 169; - public static final String GIT_SHA = "aaef829ad2830d39cc9f9e05e61e973658d7784d"; - public static final String GIT_DATE = "2025-07-15 09:33:40 MDT"; + public static final int GIT_REVISION = 170; + public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d"; + public static final String GIT_DATE = "2025-07-15 10:24:11 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 09:49:13 MDT"; - public static final long BUILD_UNIX_TIME = 1752594553460L; + public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT"; + public static final long BUILD_UNIX_TIME = 1752599069523L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 3157bae..5c17506 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import org.littletonrobotics.junction.AutoLog; + import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -25,24 +27,29 @@ public class Elevator extends SubsystemBase implements Queryable { private TalonFX endeffectorMotor; private LED led; - @SuppressWarnings("unused") - private long wait = 0; - private long maxWait = 1000; + // @AutoLog + // private class ElevatorState { + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; - private double elevatorRefrence = 0; - private double endeffectorRefrence = 0; + public double elevatorRefrence = 0; + public double endeffectorRefrence = 0; - private boolean elevatorManualStop = true; - private boolean endefectorManualStop = true; + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; - private boolean disableAutoIntake = false; + public boolean disableAutoIntake = false; - private boolean seededZeroEndefector = false; - private boolean seededZeroElevator = false; + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; - private DigitalInput basinBeamBreak; - private DigitalInput endeffectorLimitSwitch; - private DigitalInput intakeIR; + public DigitalInput basinBeamBreak; + public DigitalInput endeffectorLimitSwitch; + public DigitalInput intakeIR; + // } + + // private ElevatorState state = new ElevatorState(); public enum CoordinationState { Waiting, // for coral into the though diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index fceffe4..1e86ae2 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -60,11 +60,6 @@ public class LED extends SubsystemBase implements Queryable { return "LEDs"; } - // @Override - // public void queryStatus() { - // SmartDashboard.putString("LED status", mode.name()); - // } - @Override public Status diagnosticStatus() { Status status = new Status(); diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index 6981d7a..37ec4de 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -51,10 +51,6 @@ public class Lidar extends SubsystemBase implements Queryable { return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; } - ShuffleboardLayout subsystemLayout; - GenericEntry sbDistance; - GenericEntry sbWithinDistance; - @Override public String getName() { return "Lidar " + name; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4a11b1b..17bf2dd 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; @@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.DriveConstants; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -40,27 +42,30 @@ import com.pathplanner.lib.config.RobotConfig; public class SwerveDrive extends SubsystemBase implements Queryable { private SwerveDrivetrain swerveDriveTrain; - private Vision vision; - private int gear_index = DriveConstants.STARTING_GEAR; - private boolean stopped = false; - @SuppressWarnings("unused") - private boolean robotKnowsWhereItIs = false; + @AutoLog + public class SwerveDriveState { + public int gear_index = DriveConstants.STARTING_GEAR; + public boolean stopped = false; + public boolean robotKnowsWhereItIs = false; + + public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + + public double lastOdomSpeed; + + public Pose2d initalPose2d = null; + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + } - public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to - // 25% - - public double lastOdomSpeed; - - public Pose2d initalPose2d = null; - - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + public SwerveDriveState state = new SwerveDriveState(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { @@ -81,7 +86,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) @@ -138,7 +143,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; - initalPose2d = pose; + state.initalPose2d = pose; swerveDriveTrain.resetPose(pose); } @@ -157,7 +162,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput @@ -165,7 +170,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - stopped = false; + state.stopped = false; if (fieldRelative) { leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -173,18 +178,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // ! drift correction if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) - .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget)); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, @@ -197,20 +202,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(-leftStick.getY() * speedAdjust) - .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(-leftStick.getY() * state.speedAdjust) + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); } } public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { - stopped = false; + state.stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); } @@ -220,7 +225,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // relitive version of // this, and no pre // provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve // drive is still going: stopModules(); // stop the swerve @@ -230,8 +235,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(rightStick.getAngle())); } @@ -239,8 +244,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -254,8 +259,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(heading); // ctrl.HeadingController.setPID( // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -298,6 +303,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return false; } + public boolean isStopped() { + return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY; + } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the // swerve drive is still going: @@ -309,8 +318,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * -speedAdjust) - .withVelocityY(leftStick.getY() * speedAdjust) + .withVelocityX(leftStick.getX() * -state.speedAdjust) + .withVelocityY(leftStick.getY() * state.speedAdjust) .withTargetDirection(rot)); // double } @@ -320,19 +329,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d); } public void resetGyro() { swerveDriveTrain.tareEverything(); - robotKnowsWhereItIs = false; - rotTarget = 0; + state.robotKnowsWhereItIs = false; + state.rotTarget = 0; // vision.resetRotations(); } public void softStop() { - stopped = true; + state.stopped = true; swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) @@ -350,7 +359,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void periodic() { // This method will be called once per scheduler run\ SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); - SmartDashboard.putNumber("RotTartget", rotTarget); + SmartDashboard.putNumber("RotTartget", state.rotTarget); double time = Vision.getTime(); double freq = swerveDriveTrain.getOdometryFrequency(); @@ -367,82 +376,80 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } private void reset_index() { - gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index - 1; + int i = state.gear_index - 1; if (i == -1) i = 0; setPercentOutput(DriveConstants.GEARS[i]); - gear_index = i; + state.gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = gear_index + 1; + int i = state.gear_index + 1; if (i == DriveConstants.GEARS.length) i = DriveConstants.GEARS.length - 1; setPercentOutput(DriveConstants.GEARS[i]); - gear_index = i; + state.gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - gear_index = -1; + state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + state.gear_index = -1; } public void setToSlow() { setPercentOutput(DriveConstants.SLOW_SPEED); - gear_index = 0; + state.gear_index = 0; } public void setToFast() { setPercentOutput(DriveConstants.FAST_SPEED); - gear_index = 1; + state.gear_index = 1; } public void setToTurbo() { setPercentOutput(DriveConstants.TURBO_SPEED); - gear_index = 2; + state.gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; } private int tmp_gear_index = DriveConstants.STARTING_GEAR; public void startSlowPeriod() { - tmp_gear_index = gear_index; + tmp_gear_index = state.gear_index; setToSlow(); } public void startTurboPeriod() { - tmp_gear_index = gear_index; + tmp_gear_index = state.gear_index; setToTurbo(); } public void endSlowPeriod() { setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); - gear_index = tmp_gear_index; + state.gear_index = tmp_gear_index; } public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ - this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - - SmartDashboard.putNumber("Speed", lastOdomSpeed); + state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; } } @@ -453,28 +460,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return "Swerve Drive Controller"; } - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); - - GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); - - // @Override - // public void queryStatus() { - // sbGyro.setDouble(getGyroAngle()); - // sbShiftState.setDouble(this.speedAdjust); - - // // TODO: Add more status things - // } - @Override public Status diagnosticStatus() { Status status = new Status(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b11c501..96753a0 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -10,6 +10,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -35,44 +36,22 @@ import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; public class Vision extends SubsystemBase implements Queryable { - - // private PhotonCamera leftCamera; - // private PhotonCamera rightCamera; - private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; - private List poses = new ArrayList<>(); - private boolean isTagDetected = false; - private boolean isTagProcessed = false; - - private double lastLatency = 0; - - public double getLastLatency() { - return lastLatency; + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + public List poses = new ArrayList<>(); + public double latency = 0; + public Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastPhysOdomPose = new Pose2d(); } - public Pose2d lastVisionPose = new Pose2d(); - private Pose2d lastPhysOdomPose = new Pose2d(); - - private Matrix curStdDevs; + private VisionState state = new VisionState(); private Field2d field = new Field2d(); - - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbTagDetected = subsystemLayout - .add("Tag Detected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbTagProcessed = subsystemLayout - .add("Tag Processed", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ FaultReporter.register(this); SmartDashboard.putData(field); @@ -98,17 +77,17 @@ public class Vision extends SubsystemBase implements Queryable { private void update() { - isTagProcessed = false; - isTagDetected = false; + state.isTagProcessed = false; + state.isTagDetected = false; // Instant now = Instant.now(); // int cams = 0; - // double latency = 0; + double latency = 0; // Pose2d pose = null; - poses.clear(); + state.poses.clear(); for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -122,9 +101,9 @@ public class Vision extends SubsystemBase implements Queryable { var result = results.get(results.size()-1); - // latency += result.getTimestampSeconds(); + latency += result.getTimestampSeconds(); - isTagDetected = isTagDetected | result.hasTargets(); + state.isTagDetected = state.isTagDetected | result.hasTargets(); // If there are no tags if(!result.hasTargets()) @@ -136,7 +115,7 @@ public class Vision extends SubsystemBase implements Queryable { if(estimatedRobotPose.isEmpty()) continue; - poses.add(estimatedRobotPose.get()); + state.poses.add(estimatedRobotPose.get()); // if(pose == null) // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); @@ -150,7 +129,7 @@ public class Vision extends SubsystemBase implements Queryable { // Yaw += (pose.getRotation().getDegrees() + 180) % 360; // cams++; - isTagProcessed = true; + state.isTagProcessed = true; } @@ -244,22 +223,13 @@ public class Vision extends SubsystemBase implements Queryable { // } // } - /** - * Returns the latest standard deviations of the estimated pose from {@link - * #getEstimatedGlobalPose()}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. - */ - public Matrix getEstimationStdDevs() { - return curStdDevs; - } public void setLastOdomPose(Optional pose){ if(pose.isPresent()) - lastPhysOdomPose = pose.get(); + state.lastPhysOdomPose = pose.get(); } // public double getLastOdomSpeed(){ @@ -267,8 +237,8 @@ public class Vision extends SubsystemBase implements Queryable { // } public Pose2d getPose2d() { - if(lastPhysOdomPose != null) - return lastPhysOdomPose; + if(state.lastPhysOdomPose != null) + return state.lastPhysOdomPose; // if(lastVisionPose != null) // return lastVisionPose; @@ -281,12 +251,12 @@ public class Vision extends SubsystemBase implements Queryable { } public boolean isTag(){ - return isTagDetected && isTagProcessed; + return state.isTagDetected && state.isTagProcessed; } public void addVisionMeasurement( SwerveDrivetrain drivetrain){ - for(EstimatedRobotPose pose : poses){ + for(EstimatedRobotPose pose : state.poses){ drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); } } @@ -303,19 +273,6 @@ public class Vision extends SubsystemBase implements Queryable { return "Vision"; } -// GenericEntry sbShiftState = subsystemLayout -// .add("Shift State", 0) -// .withWidget(BuiltInWidgets.kNumberBar) -// .getEntry(); - - - // @Override - // public void queryStatus() { - // sbTagDetected.setBoolean(isTagDetected); - // sbTagProcessed.setBoolean(isTagProcessed); - // // field.setRobotPose(getPose2d()); - // } - @Override public Status diagnosticStatus() { return new Status(); diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java new file mode 100644 index 0000000..c8e9c1c --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,14 @@ +package frc4388.utility.status; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc4388.robot.RobotContainer; + +// Class to update a series of WPILIB Alerts +public class Alerts { + private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); + + public static void UpdateAlerts(RobotContainer m_RobotContainer) { + no_auto.set(!m_RobotContainer.autoChooserUpdated); + } +} From 8e34bfe35427e1fb86ab2af21dd706d387bf4874 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 15 Jul 2025 12:42:25 -0700 Subject: [PATCH 08/10] Work on adding advantagekit --- src/main/java/frc4388/robot/Robot.java | 2 - .../java/frc4388/robot/RobotContainer.java | 7 +- src/main/java/frc4388/robot/RobotMap.java | 25 +- .../robot/commands/alignment/DriveToReef.java | 4 +- .../commands/alignment/DriveUntilLiDAR.java | 6 +- .../robot/commands/alignment/LidarAlign.java | 6 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/Elevator.java | 2 - .../java/frc4388/robot/subsystems/LED.java | 1 - .../java/frc4388/robot/subsystems/Lidar.java | 71 ----- .../frc4388/robot/subsystems/SwerveDrive.java | 160 +++++----- .../java/frc4388/robot/subsystems/Vision.java | 281 ------------------ .../frc4388/robot/subsystems/lidar/LiDAR.java | 55 ++++ .../robot/subsystems/lidar/LidarIO.java | 13 + .../robot/subsystems/lidar/LidarLiteV2.java | 27 ++ .../robot/subsystems/vision/Vision.java | 94 ++++++ .../robot/subsystems/vision/VisionIO.java | 22 ++ .../subsystems/vision/VisionPhotonvision.java | 158 ++++++++++ .../java/frc4388/utility/status/Alerts.java | 6 +- 19 files changed, 483 insertions(+), 467 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Lidar.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/VisionIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 610f140..0a31049 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,9 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3608ad8..aeb3808 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -55,11 +55,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.Elevator.CoordinationState; // import frc4388.robot.subsystems.Endeffector; import frc4388.robot.subsystems.SwerveDrive; - +import frc4388.robot.subsystems.vision.Vision; // Utilites import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; @@ -695,8 +694,8 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown())); new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;})) - .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;})); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;})) + .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;})); new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod())) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 00be570..1e0cb87 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -21,9 +21,12 @@ import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.VisionConstants; import frc4388.robot.constants.DriveConstants; -import frc4388.robot.subsystems.Lidar; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.lidar.LidarIO; +import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.vision.VisionIO; +import frc4388.robot.subsystems.vision.VisionPhotonvision; import frc4388.utility.status.FaultCANCoder; -import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -35,11 +38,13 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS); + public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS); - public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef"); - public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse"); + // public final LiDAR lidar = new + + public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef"); + public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse"); /* LED Subsystem */ @@ -69,8 +74,8 @@ public class RobotMap { public RobotMap() { configureDriveMotorControllers(); - FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); - FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); + // FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); + // FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro"); @@ -111,8 +116,8 @@ public class RobotMap { cameraProp.setAvgLatencyMs(35); cameraProp.setLatencyStdDevMs(5); - sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); - sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); sim.leftCamera.enableRawStream(true); diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 11a1f82..3d13052 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.robot.subsystems.Vision; +import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.ReefPositionHelper; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.compute.ReefPositionHelper.Side; @@ -131,7 +131,7 @@ public class DriveToReef extends Command { (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) + (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) ); // System.out.println(finished); diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index b3b35a4..d6ca489 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -2,22 +2,22 @@ package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { private final SwerveDrive swerveDrive; private final Translation2d leftStick; private final Translation2d rightStick; - private final Lidar m_lidar; + private final LiDAR m_lidar; private final double mindistance; public DriveUntilLiDAR( SwerveDrive swerveDrive, Translation2d leftStick, Translation2d rightStick, - Lidar lidar, + LiDAR lidar, double mindistance) { addRequirements(swerveDrive); diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index c40ca26..668d3fa 100644 --- a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -8,13 +8,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LidarAlign extends Command { private SwerveDrive swerveDrive; - private Lidar lidar; + private LiDAR lidar; private int currentFinderTick; // private int tickFoundPipe; @@ -26,7 +26,7 @@ public class LidarAlign extends Command { // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ - public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) { + public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) { // Use addRequirements() here to declare subsystem dependencies. this.swerveDrive = swerveDrive; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ae418b7..f131ea0 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 170; - public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d"; - public static final String GIT_DATE = "2025-07-15 10:24:11 MDT"; + public static final int GIT_REVISION = 171; + public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6"; + public static final String GIT_DATE = "2025-07-15 11:07:01 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT"; - public static final long BUILD_UNIX_TIME = 1752599069523L; + public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT"; + public static final long BUILD_UNIX_TIME = 1752608435113L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 5c17506..7ac9418 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -4,8 +4,6 @@ package frc4388.robot.subsystems; -import org.littletonrobotics.junction.AutoLog; - import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 1e86ae2..c050278 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -11,7 +11,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.LEDConstants; import frc4388.utility.status.Status; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java deleted file mode 100644 index 37ec4de..0000000 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ /dev/null @@ -1,71 +0,0 @@ -package frc4388.robot.subsystems; - -import org.littletonrobotics.junction.AutoLogOutput; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.Counter; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.Constants.LiDARConstants; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; -import frc4388.utility.status.Status.ReportLevel; - -// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class Lidar extends SubsystemBase implements Queryable { - - private Counter LidarPWM; - private String name = "Lidar"; - - private double distance = -1; - public Lidar(int port, String name) { - FaultReporter.register(this); - - this.name = name; - LidarPWM = new Counter(port); - LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured - LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement - LidarPWM.reset(); - - } - - @Override - public void periodic() { - if(LidarPWM.get() < 1) - distance = -1; - else - distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; - } - - @AutoLogOutput(key = "Lidar/{name}") - public double getDistance(){ - return distance; - } - - public boolean withinDistance(){ - if(distance == -1) return false; - return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; - } - - @Override - public String getName() { - return "Lidar " + name; - } - - @Override - public Status diagnosticStatus() { - Status s = new Status(); - - if(distance == -1) - s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); - - s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance); - - return s; - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 17bf2dd..e37de34 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,7 +6,6 @@ package frc4388.robot.subsystems; import java.util.Optional; -import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; @@ -20,15 +19,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; @@ -44,28 +39,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable { private SwerveDrivetrain swerveDriveTrain; private Vision vision; - @AutoLog - public class SwerveDriveState { - public int gear_index = DriveConstants.STARTING_GEAR; - public boolean stopped = false; - public boolean robotKnowsWhereItIs = false; - - public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to - // 25% - - public double lastOdomSpeed; - - public Pose2d initalPose2d = null; - - - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - } + // @AutoLog + // public class SwerveDriveState { + public int gear_index = DriveConstants.STARTING_GEAR; + public boolean stopped = false; + public boolean robotKnowsWhereItIs = false; - public SwerveDriveState state = new SwerveDriveState(); + public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + // 25% + + public double lastOdomSpeed; + + public Pose2d initalPose2d = null; + + + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + // } + + // public SwerveDriveState state = new SwerveDriveState(); /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { @@ -86,7 +81,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d); + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) @@ -135,7 +130,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // null, // null, // null, - // (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), + // (state) -> Logger.recordOutput("Drive/SysIdState", toString())), // new SysIdRoutine.Mechanism( // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); @@ -143,7 +138,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; - state.initalPose2d = pose; + initalPose2d = pose; swerveDriveTrain.resetPose(pose); } @@ -162,7 +157,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // } public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going: + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput @@ -170,7 +165,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - state.stopped = false; + stopped = false; if (fieldRelative) { leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -178,18 +173,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // ! drift correction if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); + rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) - .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, @@ -202,20 +197,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } else { // Create robot-relative speeds. swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(-leftStick.getY() * state.speedAdjust) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(-leftStick.getY() * speedAdjust) + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } } public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) { - state.stopped = false; + stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust)); + .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } @@ -225,7 +220,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // relitive version of // this, and no pre // provided version - if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve + if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve // drive is still going: stopModules(); // stop the swerve @@ -235,8 +230,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); } @@ -244,8 +239,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -259,8 +254,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(heading); var ctrl = new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); // ctrl.HeadingController.setPID( // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, @@ -304,7 +299,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public boolean isStopped() { - return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY; + return lastOdomSpeed < AutoConstants.STOP_VELOCITY; } public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { @@ -318,8 +313,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() - .withVelocityX(leftStick.getX() * -state.speedAdjust) - .withVelocityY(leftStick.getY() * state.speedAdjust) + .withVelocityX(leftStick.getX() * -speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); // double } @@ -329,19 +324,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d); + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); } public void resetGyro() { swerveDriveTrain.tareEverything(); - state.robotKnowsWhereItIs = false; - state.rotTarget = 0; + robotKnowsWhereItIs = false; + rotTarget = 0; // vision.resetRotations(); } public void softStop() { - state.stopped = true; + stopped = true; swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) @@ -359,7 +354,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void periodic() { // This method will be called once per scheduler run\ SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); - SmartDashboard.putNumber("RotTartget", state.rotTarget); + SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime(); double freq = swerveDriveTrain.getOdometryFrequency(); @@ -370,86 +365,95 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - // if (vision.isTag()) {5 + if (vision.isTag()) { + Pose2d pose = vision.getPose2d(); + if (!robotKnowsWhereItIs) { + robotKnowsWhereItIs = true; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); + } + + vision.addVisionMeasurement(swerveDriveTrain); + } // if(e.isPresent()) } private void reset_index() { - state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = state.gear_index - 1; + int i = gear_index - 1; if (i == -1) i = 0; setPercentOutput(DriveConstants.GEARS[i]); - state.gear_index = i; + gear_index = i; } public void shiftUp() { - if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index - int i = state.gear_index + 1; + int i = gear_index + 1; if (i == DriveConstants.GEARS.length) i = DriveConstants.GEARS.length - 1; setPercentOutput(DriveConstants.GEARS[i]); - state.gear_index = i; + gear_index = i; } public void setPercentOutput(double speed) { - state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; - state.gear_index = -1; + speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + gear_index = -1; } public void setToSlow() { setPercentOutput(DriveConstants.SLOW_SPEED); - state.gear_index = 0; + gear_index = 0; } public void setToFast() { setPercentOutput(DriveConstants.FAST_SPEED); - state.gear_index = 1; + gear_index = 1; } public void setToTurbo() { setPercentOutput(DriveConstants.TURBO_SPEED); - state.gear_index = 2; + gear_index = 2; } public void shiftUpRot() { - state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + rotSpeedAdjust = DriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; } private int tmp_gear_index = DriveConstants.STARTING_GEAR; public void startSlowPeriod() { - tmp_gear_index = state.gear_index; + tmp_gear_index = gear_index; setToSlow(); } public void startTurboPeriod() { - tmp_gear_index = state.gear_index; + tmp_gear_index = gear_index; setToTurbo(); } public void endSlowPeriod() { setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); - state.gear_index = tmp_gear_index; + gear_index = tmp_gear_index; } public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ - state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 96753a0..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,281 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; - -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -import org.littletonrobotics.junction.AutoLog; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.Constants.FieldConstants; -import frc4388.robot.constants.Constants.VisionConstants; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; - -public class Vision extends SubsystemBase implements Queryable { - private PhotonCamera[] cameras; - private PhotonPoseEstimator[] estimators; - - @AutoLog - public class VisionState { - public boolean isTagDetected = false; - public boolean isTagProcessed = false; - public List poses = new ArrayList<>(); - public double latency = 0; - public Pose2d lastVisionPose = new Pose2d(); - public Pose2d lastPhysOdomPose = new Pose2d(); - } - - private VisionState state = new VisionState(); - - private Field2d field = new Field2d(); - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ - FaultReporter.register(this); - SmartDashboard.putData(field); - - this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; - - PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); - PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); - - photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; - } - - @Override - public void periodic() { - update(); - field.setRobotPose(getPose2d()); - } - - // private Instant lastVisionTime = null; - - - private void update() { - state.isTagProcessed = false; - state.isTagDetected = false; - - // Instant now = Instant.now(); - - // int cams = 0; - - double latency = 0; - - // Pose2d pose = null; - state.poses.clear(); - - for(int i = 0; i < cameras.length; i++){ - PhotonCamera camera = cameras[i]; - PhotonPoseEstimator estimator = estimators[i]; - - var results = camera.getAllUnreadResults(); - - // If there are no more updates from the camera - if (results.size() == 0) - continue; - - - var result = results.get(results.size()-1); - latency += result.getTimestampSeconds(); - - state.isTagDetected = state.isTagDetected | result.hasTargets(); - - // If there are no tags - if(!result.hasTargets()) - continue; - - Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); - - // If the tag was failed to be processed - if(estimatedRobotPose.isEmpty()) - continue; - - state.poses.add(estimatedRobotPose.get()); - - // if(pose == null) - // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - // else - // pose = pose.interpolate(pose, 0.5); - // X += pose.getX(); - // Y += pose.getY(); - - // if(X > 6) - - // Yaw += (pose.getRotation().getDegrees() + 180) % 360; - // cams++; - - state.isTagProcessed = true; - - - } - } - - - /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should - * only be called once per loop. - * - *

Also includes updates for the standard deviations, which can (optionally) be retrieved with - * {@link getEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets - * used for estimation. - */ - public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { - Optional visionEst = Optional.empty(); - - var targets = change.getTargets(); - for(int i = targets.size()-1; i >= 0; i--){ - Transform3d pos = targets.get(i).getBestCameraToTarget(); - double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); - if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { - change.targets.remove(i); - } - } - - if(targets.size() <= 0) - return visionEst; // Will be empty - - visionEst = estimator.update(change); - // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); - - return visionEst; - } - - - // /** - // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard - // * deviations based on number of tags, estimation strategy, and distance from the tags. - // * - // * @param estimatedPose The estimated pose to guess standard deviations for. - // * @param targets All targets in this camera frame - // */ - // private void updateEstimationStdDevs( - // Optional estimatedPose, - // List targets, - // PhotonPoseEstimator estimator) { - // if (estimatedPose.isEmpty()) { - // // No pose input. Default to single-tag std devs - // curStdDevs = VisionConstants.kSingleTagStdDevs; - - // } else { - // // Pose present. Start running Heuristic - // var estStdDevs = VisionConstants.kSingleTagStdDevs; - // int numTags = 0; - // double avgDist = 0; - - // // Precalculation - see how many tags we found, and calculate an average-distance metric - // for (var tgt : targets) { - // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - // if (tagPose.isEmpty()) continue; - - // double distance = tagPose - // .get() - // .toPose2d() - // .getTranslation() - // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - - // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - // numTags++; - // avgDist += distance; - // } - // } - - // if (numTags == 0) { - // // No tags visible. Default to single-tag std devs - // curStdDevs = VisionConstants.kSingleTagStdDevs; - // } else { - // // One or more tags visible, run the full heuristic. - // avgDist /= numTags; - // // Decrease std devs if multiple targets are visible - // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; - // // Increase std devs based on (average) distance - // if (numTags == 1 && avgDist > 4) - // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - // curStdDevs = estStdDevs; - // } - // } - // } - - - - - - public void setLastOdomPose(Optional pose){ - if(pose.isPresent()) - state.lastPhysOdomPose = pose.get(); - } - - // public double getLastOdomSpeed(){ - // return lastOdomSpeed; - // } - - public Pose2d getPose2d() { - if(state.lastPhysOdomPose != null) - return state.lastPhysOdomPose; - - // if(lastVisionPose != null) - // return lastVisionPose; - return new Pose2d(); - - } - - public static double getTime() { - return Utils.getCurrentTimeSeconds(); - } - - public boolean isTag(){ - return state.isTagDetected && state.isTagProcessed; - } - - - public void addVisionMeasurement( SwerveDrivetrain drivetrain){ - for(EstimatedRobotPose pose : state.poses){ - drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); - } - } - - - - - - - - - @Override - public String getName() { - return "Vision"; - } - - @Override - public Status diagnosticStatus() { - return new Status(); - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java new file mode 100644 index 0000000..dbbb024 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -0,0 +1,55 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; + +public class LiDAR extends SubsystemBase implements Queryable { + LidarIO io; + LidarStateAutoLogged state = new LidarStateAutoLogged(); + + private String name = "Lidar"; + + public LiDAR(LidarIO device, String name) { + FaultReporter.register(this); + + this.io = device; + this.name = name; + } + + @Override + public void periodic() { + io.updateInputs(state); + } + + // @AutoLogOutput(key = "Lidar/{name}") + public double getDistance(){ + return state.distance; + } + + public boolean withinDistance(){ + if(state.distance == -1) return false; + return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE; + } + + @Override + public String getName() { + return "Lidar " + name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(state.distance == -1) + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance); + + return s; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java new file mode 100644 index 0000000..e3b4ebc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java @@ -0,0 +1,13 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.AutoLog; + +public interface LidarIO { + @AutoLog + public class LidarState { + public boolean connected; + public double distance; + } + + public default void updateInputs(LidarState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java new file mode 100644 index 0000000..3851050 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj.Counter; +import frc4388.robot.constants.Constants.LiDARConstants; + +// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface +public class LidarLiteV2 implements LidarIO { + + + private Counter LidarPWM; + + public LidarLiteV2(int port) { + LidarPWM = new Counter(port); + LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured + LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement + LidarPWM.reset(); + } + + @Override + public void updateInputs(LidarState state) { + + if(LidarPWM.get() < 1) + state.distance = -1; + else + state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..d24ee3f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,94 @@ +package frc4388.robot.subsystems.vision; + +import java.util.Optional; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; + +public class Vision extends SubsystemBase implements Queryable { + VisionIO[] io; + VisionStateAutoLogged[] state; + + + public Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastPhysOdomPose = new Pose2d(); + + public Vision(VisionIO... devices) { + FaultReporter.register(this); + io = devices; + state = new VisionStateAutoLogged[io.length]; + + for(int i = 0; i < io.length; i++) { + state[i] = new VisionStateAutoLogged(); + } + } + + @Override + public void periodic() { + for(int i = 0; i < io.length; i++) { + io[i].updateInputs(state[i]); + Logger.processInputs("Vision/Camera" + i , state[i]); + } + } + + public void addVisionMeasurement(SwerveDrivetrain drivetrain){ + // for(EstimatedRobotPose pose : poses){ + // + // } + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + PoseObservation pose = state[i].lastEstimatedPose; + drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + } + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + public boolean isTag(){ + for(int i = 0; i < state.length; i++){ + if(state[i].isTagDetected && state[i].isTagProcessed) + return true; + } + return false; + } + + @AutoLogOutput + public Pose2d getPose2d() { + if(lastPhysOdomPose != null) + return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; + return new Pose2d(); + + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + @Override + public Status diagnosticStatus() { + return new Status(); + // // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..f979135 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,22 @@ +package frc4388.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO { + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + // public double latency = 0; + public PoseObservation lastEstimatedPose = null; + } + + public static record PoseObservation( + Pose2d pose, + double timestamp + ) {} + + public default void updateInputs(VisionState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java new file mode 100644 index 0000000..888dc12 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java @@ -0,0 +1,158 @@ +package frc4388.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.Constants.VisionConstants; + +public class VisionPhotonvision implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionPhotonvision(PhotonCamera camera, Transform3d position){ + this.camera = camera; + estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); + estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + // private Instant lastVisionTime = null; + + + public void updateInputs(VisionState state) { + state.isTagProcessed = false; + state.isTagDetected = false; + + state.lastEstimatedPose = null; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + return; + + + var result = results.get(results.size()-1); + + state.isTagDetected = state.isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + return; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + return; + + EstimatedRobotPose pose = estimatedRobotPose.get(); + + state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds); + + state.isTagProcessed = true; + } + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { + Optional visionEst = Optional.empty(); + + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); + } + } + + if(targets.size() <= 0) + return visionEst; // Will be empty + + visionEst = estimator.update(change); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + + return visionEst; + } + + public String getName() { + return camera.getName(); + } + + + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; + + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; + + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } + + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } +} diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java index c8e9c1c..4065586 100644 --- a/src/main/java/frc4388/utility/status/Alerts.java +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -6,9 +6,5 @@ import frc4388.robot.RobotContainer; // Class to update a series of WPILIB Alerts public class Alerts { - private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); - - public static void UpdateAlerts(RobotContainer m_RobotContainer) { - no_auto.set(!m_RobotContainer.autoChooserUpdated); - } + public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); } From 2b7bb1224195b9e001e60b895ee04d63abdfc513 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 17 Jul 2025 09:15:19 -0600 Subject: [PATCH 09/10] Attempt to implement advantagekit for CTRE swerve --- src/main/java/frc4388/robot/Robot.java | 12 +- .../java/frc4388/robot/RobotContainer.java | 28 +-- src/main/java/frc4388/robot/RobotMap.java | 160 +++++++++++------- .../robot/commands/MoveForTimeCommand.java | 2 +- .../robot/commands/MoveUntilSuply.java | 2 +- .../robot/commands/alignment/DriveToReef.java | 2 +- .../commands/alignment/DriveUntilLiDAR.java | 2 +- .../robot/commands/alignment/LidarAlign.java | 2 +- .../robot/commands/swerve/RotateToAngle.java | 2 +- .../commands/swerve/neoJoystickPlayback.java | 2 +- .../commands/swerve/neoJoystickRecorder.java | 2 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/DiffDrive.java | 4 +- .../frc4388/robot/subsystems/lidar/LiDAR.java | 3 + .../subsystems/{ => swerve}/SwerveDrive.java | 158 ++++++++--------- .../swerve/SwerveDriveConstants.java} | 4 +- .../robot/subsystems/swerve/SwerveIO.java | 33 ++++ .../subsystems/swerve/SwervePhoenix.java | 65 +++++++ .../robot/subsystems/vision/Vision.java | 19 ++- .../utility/compute/TimesNegativeOne.java | 18 +- 20 files changed, 326 insertions(+), 204 deletions(-) rename src/main/java/frc4388/robot/subsystems/{ => swerve}/SwerveDrive.java (73%) rename src/main/java/frc4388/robot/{constants/DriveConstants.java => subsystems/swerve/SwerveDriveConstants.java} (99%) create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0a31049..c147692 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -205,13 +205,13 @@ public class Robot extends LoggedRobot { // } - @Override - public void simulationPeriodic() { - m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); - // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); + // @Override + // public void simulationPeriodic() { + // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); + // // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); - // m_robotContainer.m_robotSwerveDrive. - } + // // m_robotContainer.m_robotSwerveDrive. + // } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aeb3808..4a6f4d8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -48,6 +48,7 @@ import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.OIConstants; +import frc4388.robot.constants.Constants.SimConstants.Mode; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -56,8 +57,8 @@ import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Elevator.CoordinationState; -// import frc4388.robot.subsystems.Endeffector; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; // Utilites import frc4388.utility.DeferredBlock; @@ -74,7 +75,7 @@ import frc4388.utility.compute.ReefPositionHelper.Side; public class RobotContainer { /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(Mode.REAL); /* Subsystems */ public final LED m_robotLED = new LED(); @@ -83,6 +84,9 @@ public class RobotContainer { public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); + public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); + public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -129,7 +133,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -292,7 +296,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -339,7 +343,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -380,7 +384,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -409,7 +413,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -429,7 +433,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -450,7 +454,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -558,7 +562,7 @@ public class RobotContainer { NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); + new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped())); @@ -722,7 +726,7 @@ public class RobotContainer { .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_robotMap.reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1e0cb87..45eb62a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -19,14 +19,18 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DigitalInput; import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; -import frc4388.robot.constants.DriveConstants; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.lidar.LidarIO; import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO; +import frc4388.robot.subsystems.swerve.SwervePhoenix; import frc4388.robot.subsystems.vision.VisionIO; import frc4388.robot.subsystems.vision.VisionPhotonvision; import frc4388.utility.status.FaultCANCoder; +import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -38,24 +42,20 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS); - public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS); + public final VisionIO leftCamera; + public final VisionIO rightCamera; // public final LiDAR lidar = new - public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef"); - public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse"); + public final LidarIO reefLidar; + public final LidarIO reverseLidar; /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); /* Swreve Drive Subsystem */ - public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - DriveConstants.DrivetrainConstants, - DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT, - DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT - ); + public final SwerveIO swerveDrivetrain; /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); @@ -66,71 +66,101 @@ public class RobotMap { public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - void configureDriveMotorControllers() { - // endeffector.saf + public RobotMap(SimConstants.Mode mode) { + switch (mode) { + case REAL: + // Configure cameras + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + + leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + + // Configure LiDAR + reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + + // Configure swerve drive train + + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); + + swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal); + + + + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + + FaultTalonFX.addDevice(elevator, "Elevator"); + FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + + break; + // case SIM: + // break; + default: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + reefLidar = new LidarIO() {}; + reverseLidar = new LidarIO() {}; + swerveDrivetrain = new SwerveIO() {}; + break; + } } + // public class RobotMapSim { + // public PhotonCameraSim leftCamera; + // public PhotonCameraSim rightCamera; + // } - public RobotMap() { - configureDriveMotorControllers(); + // public RobotMapSim configureSim() { + // RobotMapSim sim = new RobotMapSim(); - // FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); - // FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); + // // The simulated camera properties + // SimCameraProperties cameraProp = new SimCameraProperties(); + // // A 640 x 480 camera with a 100 degree diagonal FOV. + // cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); + // // Approximate detection noise with average and standard deviation error in pixels. + // cameraProp.setCalibError(0.25, 0.08); + // // Set the camera image capture framerate (Note: this is limited by robot loop rate). + // cameraProp.setFPS(20); + // // The average and standard deviation in milliseconds of image data latency. + // cameraProp.setAvgLatencyMs(35); + // cameraProp.setLatencyStdDevMs(5); - FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro"); - - FaultTalonFX.addDevice(elevator, "Elevator"); - FaultTalonFX.addDevice(endeffector, "Endeffector"); - - FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getDriveMotor(), "Module 0 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getSteerMotor(), "Module 0 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(0).getEncoder(), "Module 0 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getDriveMotor(), "Module 1 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getSteerMotor(), "Module 1 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(1).getEncoder(), "Module 1 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getDriveMotor(), "Module 2 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getSteerMotor(), "Module 2 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(2).getEncoder(), "Module 2 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getDriveMotor(), "Module 3 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getSteerMotor(), "Module 3 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(3).getEncoder(), "Module 3 CANCoder"); - } - - public class RobotMapSim { - public PhotonCameraSim leftCamera; - public PhotonCameraSim rightCamera; - } - - public RobotMapSim configureSim() { - RobotMapSim sim = new RobotMapSim(); - - // The simulated camera properties - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). - cameraProp.setFPS(20); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); - // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + // // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); - sim.leftCamera.enableRawStream(true); - sim.leftCamera.enableProcessedStream(true); - sim.leftCamera.enableDrawWireframe(true); + // sim.leftCamera.enableRawStream(true); + // sim.leftCamera.enableProcessedStream(true); + // sim.leftCamera.enableDrawWireframe(true); - sim.rightCamera.enableRawStream(true); - sim.rightCamera.enableProcessedStream(true); - sim.rightCamera.enableDrawWireframe(true); + // sim.rightCamera.enableRawStream(true); + // sim.rightCamera.enableProcessedStream(true); + // sim.rightCamera.enableDrawWireframe(true); - return sim; + // return sim; - } + // } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index 6a085ec..3a4e043 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -4,7 +4,7 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class MoveForTimeCommand extends Command { diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java index aa1b210..c5b5e53 100644 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -4,7 +4,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class MoveUntilSuply extends Command { diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 3d13052..6a11e85 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.ReefPositionHelper; import frc4388.utility.compute.TimesNegativeOne; diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index d6ca489..648a7d8 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -2,8 +2,8 @@ package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index 668d3fa..19efd85 100644 --- a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LidarAlign extends Command { diff --git a/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java index fea8b77..50e616c 100644 --- a/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java @@ -6,7 +6,7 @@ package frc4388.robot.commands.swerve; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class RotateToAngle extends PID { diff --git a/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java index f1c865e..bff5105 100644 --- a/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java @@ -6,7 +6,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.VirtualController; import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; diff --git a/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java index 1df53b6..4cf3159 100644 --- a/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java @@ -7,7 +7,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f131ea0..f1e17b4 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 171; - public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6"; - public static final String GIT_DATE = "2025-07-15 11:07:01 MDT"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874"; + public static final String GIT_DATE = "2025-07-15 13:42:25 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT"; - public static final long BUILD_UNIX_TIME = 1752608435113L; + public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT"; + public static final long BUILD_UNIX_TIME = 1752704433011L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index df9f29e..4aff045 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -13,7 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; // import frc4388.utility.RobotGyro; import frc4388.utility.compute.RobotTime; import frc4388.utility.status.Status; @@ -56,7 +56,7 @@ public class DiffDrive extends SubsystemBase implements Queryable { @Override public void periodic() { - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { + if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { updateSmartDashboard(); } } diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java index dbbb024..2945c2d 100644 --- a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -1,5 +1,7 @@ package frc4388.robot.subsystems.lidar; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.utility.status.Status; @@ -23,6 +25,7 @@ public class LiDAR extends SubsystemBase implements Queryable { @Override public void periodic() { io.updateInputs(state); + Logger.processInputs("LiDAR/"+name, state); } // @AutoLogOutput(key = "Lidar/{name}") diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java similarity index 73% rename from src/main/java/frc4388/robot/subsystems/SwerveDrive.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index e37de34..4a5f4cc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.swerve; import java.util.Optional; @@ -22,7 +22,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -36,18 +36,22 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; public class SwerveDrive extends SubsystemBase implements Queryable { - private SwerveDrivetrain swerveDriveTrain; + // private SwerveDrivetrain swerveDriveTrain; + + private SwerveIO io; + private SwerveStateAutoLogged state; + private Vision vision; - // @AutoLog - // public class SwerveDriveState { - public int gear_index = DriveConstants.STARTING_GEAR; + + + public int gear_index = SwerveDriveConstants.STARTING_GEAR; public boolean stopped = false; public boolean robotKnowsWhereItIs = false; - public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% public double lastOdomSpeed; @@ -58,17 +62,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - // } - - // public SwerveDriveState state = new SwerveDriveState(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain // swerveDriveTrain) { FaultReporter.register(this); - this.swerveDriveTrain = swerveDriveTrain; + this.io = swerveDriveTrain; + this.state = new SwerveStateAutoLogged(); + this.vision = vision; RobotConfig config; @@ -81,12 +84,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + return getPose2d(); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for @@ -139,7 +142,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; initalPose2d = pose; - swerveDriveTrain.resetPose(pose); + io.resetPose(pose); } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -172,12 +175,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); // ! drift correction - if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + + rotTarget = state.currentPose.getRotation().getDegrees(); + + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { @@ -186,17 +192,17 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); ctrl.HeadingController.setPID( - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); SmartDashboard.putBoolean("drift correction", true); } } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + io.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(-leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); @@ -207,9 +213,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } @@ -229,7 +235,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); @@ -243,11 +249,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { @@ -262,31 +268,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD // ); - swerveDriveTrain.setControl(ctrl); - } - - public void setLimits(double limitInAmps) { - for (SwerveModule module : swerveDriveTrain.getModules()) { - var talonFXConfigurator = module.getDriveMotor().getConfigurator(); - var talonFXConfigs = new TalonFXConfiguration(); - - talonFXConfigurator.refresh(talonFXConfigs); - talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; - talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; - talonFXConfigurator.apply(talonFXConfigs); - } + io.setControl(ctrl); } public void activateLuigiMode() { - setLimits(20); + io.setLimits(20); } public void deactivateLuigiMode() { - setLimits(DriveConstants.Configurations.SLIP_CURRENT); + io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); } public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) .withVelocityY(0) .withTargetDirection(Rotation2d.fromDegrees(angle))); @@ -312,7 +306,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); @@ -324,11 +318,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + if(state.currentPose == null) + return initalPose2d; + return state.currentPose; } public void resetGyro() { - swerveDriveTrain.tareEverything(); + io.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; // vision.resetRotations(); @@ -337,7 +333,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void softStop() { stopped = true; - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) .withRotationalRate(0) @@ -356,14 +352,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); SmartDashboard.putNumber("RotTartget", rotTarget); - double time = Vision.getTime(); - double freq = swerveDriveTrain.getOdometryFrequency(); - - Optional curpose = swerveDriveTrain.samplePoseAt(time); - Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + io.updateInputs(state); - vision.setLastOdomPose(curpose); - setLastOdomSpeed(curpose, lastPose, freq); + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -373,66 +365,66 @@ public class SwerveDrive extends SubsystemBase implements Queryable { rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); } - vision.addVisionMeasurement(swerveDriveTrain); + io.addVisionMeasurement(vision.getPosesToAdd()); } // if(e.isPresent()) } private void reset_index() { - gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index - 1; if (i == -1) i = 0; - setPercentOutput(DriveConstants.GEARS[i]); + setPercentOutput(SwerveDriveConstants.GEARS[i]); gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index + 1; - if (i == DriveConstants.GEARS.length) - i = DriveConstants.GEARS.length - 1; - setPercentOutput(DriveConstants.GEARS[i]); + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; } public void setToSlow() { - setPercentOutput(DriveConstants.SLOW_SPEED); + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); gear_index = 0; } public void setToFast() { - setPercentOutput(DriveConstants.FAST_SPEED); + setPercentOutput(SwerveDriveConstants.FAST_SPEED); gear_index = 1; } public void setToTurbo() { - setPercentOutput(DriveConstants.TURBO_SPEED); + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } - private int tmp_gear_index = DriveConstants.STARTING_GEAR; + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; public void startSlowPeriod() { tmp_gear_index = gear_index; @@ -445,15 +437,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void endSlowPeriod() { - setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; } - public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - if(curPose.isPresent() && lastPose.isPresent()){ - lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){ + if(curPose != null && lastPose != null){ + lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; } } @@ -473,11 +465,5 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return status; } - - - // Update CTRE simulation, if used. - public void updateSim(double voltage) { - swerveDriveTrain.updateSimState(0.02, voltage); - } } diff --git a/src/main/java/frc4388/robot/constants/DriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java similarity index 99% rename from src/main/java/frc4388/robot/constants/DriveConstants.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 0021eae..35b02db 100644 --- a/src/main/java/frc4388/robot/constants/DriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -1,4 +1,4 @@ -package frc4388.robot.constants; +package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; @@ -28,7 +28,7 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. -public final class DriveConstants { +public final class SwerveDriveConstants { public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..5debd5a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,33 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public interface SwerveIO { + @AutoLog + public class SwerveState { + public Pose2d currentPose = null; + public Pose2d lastPose = null; + public ChassisSpeeds speeds = null; + public double frequency = 1; + } + + public default void setControl(SwerveRequest ctrl) {} + + public default void setLimits(double limitInAmps) {} + + public default void tareEverything() {} + + public default void resetPose(Pose2d pose) {} + + public default void addVisionMeasurement(List poses) {} + + public default void updateInputs(SwerveState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java b/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java new file mode 100644 index 0000000..84bbcd9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java @@ -0,0 +1,65 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import frc4388.robot.subsystems.lidar.LidarIO.LidarState; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SwervePhoenix implements SwerveIO { + SwerveDrivetrain swerveDriveTrain; + + public SwervePhoenix(SwerveDrivetrain swerveDriveTrain) { + this.swerveDriveTrain = swerveDriveTrain; + swerveDriveTrain.getOdometryFrequency(); + } + + @Override + public void updateInputs(SwerveState state) { + double time = Vision.getTime(); + state.frequency = swerveDriveTrain.getOdometryFrequency(); + state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null); + state.speeds = swerveDriveTrain.getState().Speeds; + } + + @Override + public void setControl(SwerveRequest ctrl) { + swerveDriveTrain.setControl(ctrl); + } + + @Override + public void tareEverything() { + swerveDriveTrain.tareEverything(); + } + + @Override + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + @Override + public void addVisionMeasurement(List poses) { + for(PoseObservation pose : poses) { + swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index d24ee3f..a069af7 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -1,5 +1,7 @@ package frc4388.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; @@ -43,21 +45,20 @@ public class Vision extends SubsystemBase implements Queryable { } } - public void addVisionMeasurement(SwerveDrivetrain drivetrain){ - // for(EstimatedRobotPose pose : poses){ - // - // } + public List getPosesToAdd(){ + List poses = new ArrayList<>(); for(int i = 0; i < state.length; i++) { if(state[i].lastEstimatedPose != null) { - PoseObservation pose = state[i].lastEstimatedPose; - drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + poses.add(state[i].lastEstimatedPose); } } + + return poses; } - public void setLastOdomPose(Optional pose){ - if(pose.isPresent()) - lastPhysOdomPose = pose.get(); + public void setLastOdomPose(Pose2d pose){ + if(pose != null) + lastPhysOdomPose = pose; } public boolean isTag(){ diff --git a/src/main/java/frc4388/utility/compute/TimesNegativeOne.java b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java index 9b7da0e..2ba0d55 100644 --- a/src/main/java/frc4388/utility/compute/TimesNegativeOne.java +++ b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java @@ -7,16 +7,16 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; // Class that holds weather the drivers sticks should be inverted public class TimesNegativeOne { - public static boolean XAxis = DriveConstants.INVERT_X; - public static boolean YAxis = DriveConstants.INVERT_Y; - public static boolean RotAxis = DriveConstants.INVERT_ROTATION; + public static boolean XAxis = SwerveDriveConstants.INVERT_X; + public static boolean YAxis = SwerveDriveConstants.INVERT_Y; + public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; public static boolean isRed = false; - public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET); + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); private static boolean isRed() { Optional alliance = DriverStation.getAlliance(); @@ -26,10 +26,10 @@ public class TimesNegativeOne { public static void update(){ isRed = isRed(); - XAxis = DriveConstants.INVERT_X ^ isRed; - YAxis = DriveConstants.INVERT_Y ^ isRed; - RotAxis = DriveConstants.INVERT_ROTATION; - ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + XAxis = SwerveDriveConstants.INVERT_X ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + RotAxis = SwerveDriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); SmartDashboard.putBoolean("Is red alliance", isRed); } From 44286f81e7da4b05dddce68cf50b3d6daf12282a Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 17 Jul 2025 20:05:58 -0700 Subject: [PATCH 10/10] Add elevator to advantatekit --- .../java/frc4388/robot/RobotContainer.java | 6 +- src/main/java/frc4388/robot/RobotMap.java | 41 ++-- .../commands/wait/waitElevatorRefrence.java | 2 +- .../commands/wait/waitEndefectorRefrence.java | 2 +- .../robot/commands/wait/waitFeedCoral.java | 2 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/DiffDrive.java | 107 --------- .../subsystems/{ => elevator}/Elevator.java | 206 ++++++++---------- .../robot/subsystems/elevator/ElevatorIO.java | 34 +++ .../subsystems/elevator/ElevatorReal.java | 80 +++++++ .../{LidarLiteV2.java => LidarReal.java} | 4 +- .../robot/subsystems/swerve/SwerveDrive.java | 20 +- .../robot/subsystems/swerve/SwerveIO.java | 2 +- .../{SwervePhoenix.java => SwerveReal.java} | 10 +- ...isionPhotonvision.java => VisionReal.java} | 4 +- 15 files changed, 262 insertions(+), 268 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/DiffDrive.java rename src/main/java/frc4388/robot/subsystems/{ => elevator}/Elevator.java (53%) create mode 100644 src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java rename src/main/java/frc4388/robot/subsystems/lidar/{LidarLiteV2.java => LidarReal.java} (91%) rename src/main/java/frc4388/robot/subsystems/swerve/{SwervePhoenix.java => SwerveReal.java} (85%) rename src/main/java/frc4388/robot/subsystems/vision/{VisionPhotonvision.java => VisionReal.java} (97%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4a6f4d8..dfbd494 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -53,10 +53,10 @@ import frc4388.robot.constants.Constants.SimConstants.Mode; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Elevator.CoordinationState; +import frc4388.robot.subsystems.elevator.Elevator; +import frc4388.robot.subsystems.elevator.Elevator.CoordinationState; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; @@ -80,7 +80,7 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); - public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED); + public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 45eb62a..01cdb68 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -21,14 +21,16 @@ import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.elevator.ElevatorIO; +import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.lidar.LidarIO; -import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.lidar.LidarReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; import frc4388.robot.subsystems.swerve.SwerveIO; -import frc4388.robot.subsystems.swerve.SwervePhoenix; +import frc4388.robot.subsystems.swerve.SwerveReal; import frc4388.robot.subsystems.vision.VisionIO; -import frc4388.robot.subsystems.vision.VisionPhotonvision; +import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; @@ -58,13 +60,7 @@ public class RobotMap { public final SwerveIO swerveDrivetrain; /* Elevator Subsystem */ - public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); - - - public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); - public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); - public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + public final ElevatorIO elevatorIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { @@ -73,28 +69,40 @@ public class RobotMap { PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; - rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // Configure LiDAR - reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); - reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); // Configure swerve drive train - SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, SwerveDriveConstants.DrivetrainConstants, SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT ); - swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal); + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + + // Configure elevator + + TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); + TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + + + DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); + DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + + elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); + // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); FaultTalonFX.addDevice(elevator, "Elevator"); @@ -122,6 +130,7 @@ public class RobotMap { reefLidar = new LidarIO() {}; reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; + elevatorIO = new ElevatorIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java index 7108de5..7d2ec77 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitElevatorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java index 73fd893..1ff48cd 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitEndefectorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java index 2f66710..992879a 100644 --- a/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java +++ b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java @@ -5,7 +5,7 @@ package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitFeedCoral extends Command { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f1e17b4..5aeda6b 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 172; - public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874"; - public static final String GIT_DATE = "2025-07-15 13:42:25 MDT"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; + public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT"; - public static final long BUILD_UNIX_TIME = 1752704433011L; + public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; + public static final long BUILD_UNIX_TIME = 1752774931371L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 4aff045..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,107 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.swerve.SwerveDriveConstants; -// import frc4388.utility.RobotGyro; -import frc4388.utility.compute.RobotTime; -import frc4388.utility.status.Status; -import frc4388.utility.status.FaultReporter; -import frc4388.utility.status.Queryable; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase implements Queryable { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private TalonFX m_leftFrontMotor; - private TalonFX m_rightFrontMotor; - private TalonFX m_leftBackMotor; - private TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - // private Pigeon2 m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, Pigeon2 gyro) { - - FaultReporter.register(this); - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); - m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); - m_driveTrain = driveTrain; - // m_gyro = gyro; - } - - @Override - public void periodic() { - if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - // SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - // SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - // SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - //SmartDashboard.putData(m_gyro); - } - - - @Override - public String getName() { - return "Diff Drive"; - } - - // @Override - // public void queryStatus() { - // // TODO: Add Stuff - // } - - @Override - public Status diagnosticStatus() { - // Log("Diagnostic info for this has not been inplemented!"); //TODO - return new Status(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java similarity index 53% rename from src/main/java/frc4388/robot/subsystems/Elevator.java rename to src/main/java/frc4388/robot/subsystems/elevator/Elevator.java index 7ac9418..a054425 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java @@ -2,7 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -14,38 +17,31 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState; import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; import frc4388.utility.status.Status.ReportLevel; public class Elevator extends SubsystemBase implements Queryable { + ElevatorIO io; + ElevatorStateAutoLogged state = new ElevatorStateAutoLogged(); + /** Creates a new Elevator. */ - private TalonFX elevatorMotor; - private TalonFX endeffectorMotor; private LED led; - // @AutoLog - // private class ElevatorState { - @SuppressWarnings("unused") - public long wait = 0; - public long maxWait = 1000; + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; - public double elevatorRefrence = 0; - public double endeffectorRefrence = 0; + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; - public boolean elevatorManualStop = true; - public boolean endefectorManualStop = true; + public boolean disableAutoIntake = false; - public boolean disableAutoIntake = false; - - public boolean seededZeroEndefector = false; - public boolean seededZeroElevator = false; - - public DigitalInput basinBeamBreak; - public DigitalInput endeffectorLimitSwitch; - public DigitalInput intakeIR; - // } + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; // private ElevatorState state = new ElevatorState(); @@ -69,43 +65,15 @@ public class Elevator extends SubsystemBase implements Queryable { private CoordinationState currentState; // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { - elevatorMotor = elevatorTalonFX; - endeffectorMotor = endeffectorTalonFX; + public Elevator(ElevatorIO io, LED led) { + this.io = io; this.led = led; - this.basinBeamBreak = basinLimitSwitch; - this.endeffectorLimitSwitch = endeffectorLimitSwitch; - this.intakeIR = intakeDigitalInput; - - elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); - - elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); - endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); currentState = CoordinationState.Ready; FaultReporter.register(this); } - //PID methods - - private void PIDPosition(TalonFX motor, double position) { - if (motor == elevatorMotor) elevatorRefrence = position; - else endeffectorRefrence = position; - - var request = new PositionDutyCycle(position); - motor.setControl(request); - } - - public void elevatorStop() { - elevatorMotor.set(0); - } - - public void endeffectorStop() { - endeffectorMotor.set(0); - } - public void transitionState(CoordinationState state) { // elevatorMotor.enable(); @@ -115,98 +83,98 @@ public class Elevator extends SubsystemBase implements Queryable { switch (currentState) { case Waiting: { wait = System.currentTimeMillis() + maxWait; - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); led.setMode(LEDConstants.WAITING_PATTERN); break; } case WatingBeamTripped: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; } case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.DOWN_PATTERN); break; } case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); led.setMode(LEDConstants.READY_PATTERN); break; } case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case L2ScoreLeave: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL3Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); break; } case BallRemoverL3Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); led.setMode(LEDConstants.SCORING_PATTERN); break; } @@ -223,27 +191,25 @@ public class Elevator extends SubsystemBase implements Queryable { } public boolean elevatorAtReference() { - // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); - double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); - double diffrence = elevatorRefrence - elevatorPosition; + double diffrence = state.elevatorRefrence - state.elevatorPosition; boolean headedUp = diffrence < 0; - boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.elevatorForwardLimit && !headedUp) + ); } public boolean endeffectorAtReference() { - // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); - double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); - double diffrence = endeffectorRefrence - endeffectorPosition; + double diffrence = state.endeffectorRefrence - state.endeffectorPosition; boolean headedUp = diffrence < 0; - boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.endeffectorForwardLimit && !headedUp) + ); } // public void driveElevatorStick(Translation2d stick) { // if (stick.getNorm() > 0.05) { @@ -252,11 +218,11 @@ public class Elevator extends SubsystemBase implements Queryable { // } public boolean getEndeffectorLimit() { - return endeffectorLimitSwitch.get(); + return state.endeffectorLimitSwitch; } private void periodicWaiting() { - if (!basinBeamBreak.get()) + if (!state.basinBeamBreak) transitionState(CoordinationState.Ready); // if(!endeffectorLimitSwitch.get()) // transitionState(CoordinationState.Hovering); @@ -268,39 +234,39 @@ public class Elevator extends SubsystemBase implements Queryable { // } private void periodicReady() { - if (elevatorAtReference() && !endeffectorLimitSwitch.get()) + if (elevatorAtReference() && !state.endeffectorLimitSwitch) transitionState(CoordinationState.Hovering); - if(elevatorAtReference() && endeffectorLimitSwitch.get()) + if(elevatorAtReference() && state.endeffectorLimitSwitch) transitionState(CoordinationState.Hovering); } @SuppressWarnings("unused") private void periodicScoring() { - if (!endeffectorLimitSwitch.get()) + if (!state.endeffectorLimitSwitch) transitionState(CoordinationState.Waiting); } public void manualElevatorVel(double velocity) { if (Math.abs(velocity) > 0.1) { - elevatorMotor.set(velocity); + io.elevatorToVelocity(velocity); elevatorManualStop = false; return; } if (!elevatorManualStop) { elevatorManualStop = true; - elevatorMotor.set(0); + io.elevatorToVelocity(0); } } public void manualEndeffectorVel(double velocity) { if (Math.abs(velocity) > 0.1) { - endeffectorMotor.set(velocity); + io.endeffectorToVelocity(velocity); endefectorManualStop = false; return; } if (!endefectorManualStop) { endefectorManualStop = true; - endeffectorMotor.set(0); + io.endeffectorToVelocity(0); } } @@ -320,18 +286,21 @@ public class Elevator extends SubsystemBase implements Queryable { // This method will be called once per scheduler run // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); - SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); - SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); - SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); - SmartDashboard.putString("State", currentState.toString()); + // SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); + // SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + // SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); + // SmartDashboard.putString("State", currentState.toString()); - if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { - endeffectorMotor.setPosition(0); + io.updateInputs(state); + Logger.processInputs("Elevator", state); + + if (!seededZeroEndefector && state.endeffectorForwardLimit) { + io.endeffectorToPosition(0); seededZeroEndefector = !seededZeroEndefector; } - if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { - elevatorMotor.setPosition(0); + if (!seededZeroElevator && state.elevatorReverseLimit) { + io.endeffectorToPosition(0); seededZeroElevator = !seededZeroElevator; } @@ -345,7 +314,7 @@ public class Elevator extends SubsystemBase implements Queryable { periodicReady(); } - if(!intakeIR.get()){ + if(!state.intakeIR){ led.setMode(LEDConstants.DOWN_PATTERN); } @@ -355,6 +324,11 @@ public class Elevator extends SubsystemBase implements Queryable { // } } + @AutoLogOutput(key="Elevator/state") + public String getState() { + return currentState.toString(); + } + public boolean isL4Primed() { return currentState == CoordinationState.PrimedFour; } @@ -364,16 +338,24 @@ public class Elevator extends SubsystemBase implements Queryable { } public boolean hasCoral() { - return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); + return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch; + } + + public void elevatorStop() { + io.elevatorToVelocity(0); + } + + public void endeffectorStop() { + io.endeffectorToVelocity(0); } public boolean readyToMove() { - return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); + return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch; // return hasCoral(); } public void armShuffle(){ - if(!basinBeamBreak.get()){ + if(!state.basinBeamBreak){ //shuffle the coral with the arm until coral hits beam break } } diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..a48278c --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,34 @@ +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.subsystems.lidar.LidarIO.LidarState; + +public interface ElevatorIO { + @AutoLog + public class ElevatorState { + public double elevatorRefrence; + public double elevatorPosition; + public boolean elevatorForwardLimit; + public boolean elevatorReverseLimit; + + public double endeffectorRefrence; + public double endeffectorPosition; + public boolean endeffectorForwardLimit; + public boolean endeffectorReverseLimit; + + + public boolean basinBeamBreak; + public boolean endeffectorLimitSwitch; + public boolean intakeIR; + + } + + public default void elevatorToPosition(double position) {} + public default void endeffectorToPosition(double position) {} + public default void elevatorToVelocity(double velocity) {} + public default void endeffectorToVelocity(double velocity) {} + + public default void updateInputs(ElevatorState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java new file mode 100644 index 0000000..89d48aa --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.elevator; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.constants.Constants.ElevatorConstants; + +public class ElevatorReal implements ElevatorIO { + TalonFX elevatorMotor; + TalonFX endeffectorMotor; + + DigitalInput basinLimitSwitch; + DigitalInput endeffectorLimitSwitch; + DigitalInput intakeIR; + + + double elevatorRefrence = 0; + double endeffectorRefrence = 0; + + public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) { + this.elevatorMotor = elevatorTalonFX; + this.endeffectorMotor = endeffectorTalonFX; + + this.basinLimitSwitch = basinLimitSwitch; + this.endeffectorLimitSwitch = endeffectorLimitSwitch; + this.intakeIR = intakeIR; + + + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); + + elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); + endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); + } + + @Override + public void updateInputs(ElevatorState state) { + state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + state.elevatorRefrence = elevatorRefrence; + state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); + state.endeffectorRefrence = endeffectorRefrence; + state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; + state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; + + + state.basinBeamBreak = basinLimitSwitch.get(); + state.endeffectorLimitSwitch = endeffectorLimitSwitch.get(); + state.intakeIR = intakeIR.get(); + } + + @Override + public void elevatorToPosition(double position) { + elevatorRefrence = position; + var request = new PositionDutyCycle(position); + elevatorMotor.setControl(request); + } + + @Override + public void endeffectorToPosition(double position) { + endeffectorRefrence = position; + var request = new PositionDutyCycle(position); + endeffectorMotor.setControl(request); + } + + @Override + public void elevatorToVelocity(double velocity) { + elevatorMotor.set(velocity); + } + + + @Override + public void endeffectorToVelocity(double velocity) { + endeffectorMotor.set(velocity); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java similarity index 91% rename from src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java rename to src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java index 3851050..3bf33d5 100644 --- a/src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java @@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter; import frc4388.robot.constants.Constants.LiDARConstants; // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class LidarLiteV2 implements LidarIO { +public class LidarReal implements LidarIO { private Counter LidarPWM; - public LidarLiteV2(int port) { + public LidarReal(int port) { LidarPWM = new Counter(port); LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 4a5f4cc..bbc61e4 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -4,16 +4,9 @@ package frc4388.robot.subsystems.swerve; -import java.util.Optional; - +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -56,7 +48,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double lastOdomSpeed; - public Pose2d initalPose2d = null; + public Pose2d initalPose2d = new Pose2d(); public double rotTarget = 0.0; @@ -353,9 +345,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SmartDashboard.putNumber("RotTartget", rotTarget); io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); vision.setLastOdomPose(state.currentPose); - setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -448,6 +441,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; } } + + @AutoLogOutput(key="SwerveDrive/speed ") + public double getOdometrySpeed() { + return lastOdomSpeed; + } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index 5debd5a..fa7de1a 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -16,7 +16,7 @@ public interface SwerveIO { public Pose2d currentPose = null; public Pose2d lastPose = null; public ChassisSpeeds speeds = null; - public double frequency = 1; + public double odometryRate = 1; } public default void setControl(SwerveRequest ctrl) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java similarity index 85% rename from src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index 84bbcd9..05d9e8f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -10,15 +10,13 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Pose2d; -import frc4388.robot.subsystems.lidar.LidarIO.LidarState; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; -public class SwervePhoenix implements SwerveIO { +public class SwerveReal implements SwerveIO { SwerveDrivetrain swerveDriveTrain; - public SwervePhoenix(SwerveDrivetrain swerveDriveTrain) { + public SwerveReal(SwerveDrivetrain swerveDriveTrain) { this.swerveDriveTrain = swerveDriveTrain; swerveDriveTrain.getOdometryFrequency(); } @@ -26,9 +24,9 @@ public class SwervePhoenix implements SwerveIO { @Override public void updateInputs(SwerveState state) { double time = Vision.getTime(); - state.frequency = swerveDriveTrain.getOdometryFrequency(); + state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency(); state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); - state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null); state.speeds = swerveDriveTrain.getState().Speeds; } diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java similarity index 97% rename from src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java rename to src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 888dc12..1c12437 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult; import frc4388.robot.constants.Constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; -public class VisionPhotonvision implements VisionIO { +public class VisionReal implements VisionIO { // private PhotonCamera[] cameras; // private PhotonPoseEstimator[] estimators; @@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO { // public List poses = new ArrayList<>(); - public VisionPhotonvision(PhotonCamera camera, Transform3d position){ + public VisionReal(PhotonCamera camera, Transform3d position){ this.camera = camera; estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);