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