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..47517dc 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,7 +96,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..c147692 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,28 +7,24 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.Base64; -import java.util.List; +// Advantagekit +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +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.wpilibj.RobotController; 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.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.compute.Trim; +import frc4388.utility.status.FaultReporter; + //import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the @@ -37,7 +33,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(); @@ -49,32 +45,20 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + // Start logging with AdvantageKit + startLogging(); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - - new Thread() { - public void run() { - try{ - while(!this.isInterrupted() && this.isAlive()){ - Thread.sleep(500); - for(int i=0;i errors = new ArrayList<>(); - - // Subsystems header - System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY"))); - - for(int i=0;i< Subsystem.subsystems.size();i++){ - - Subsystem subsystem = Subsystem.subsystems.get(i); - System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":"); - Status status = subsystem.diagnosticStatus(); - - for(int a=0;a 0) { - // Errors header - System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY"))); - for(int i=0;i subsystems = new ArrayList<>(); @@ -124,7 +103,6 @@ public class RobotContainer { } // ! /* Autos */ - private String lastAutoName = "defualt.auto"; private SendableChooser autoChooser; private Command autoCommand; @@ -144,7 +122,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 +131,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, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -186,7 +164,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 +173,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 +221,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 +257,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 +285,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 +294,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, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -354,7 +332,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 +341,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, reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -398,15 +376,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, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -423,7 +401,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 +410,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, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -450,12 +428,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, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -471,12 +449,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, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -495,7 +473,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 +484,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 +504,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 +514,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,9 +562,9 @@ 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(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); - NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY)); + NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped())); NamedCommands.registerCommand("align-feed", new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, @@ -616,12 +594,14 @@ public class RobotContainer { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> { // Called on first robot enable + + DeferredBlock.addBlock(() -> { // Called on first robot enable m_robotSwerveDrive.resetGyro(); - }); - new DeferredBlockMulti(() -> { // Called on every robot enable + }, false); + DeferredBlock.addBlock(() -> { // Called on every robot enable TimesNegativeOne.update(); - }); + }, true); + DriverStation.silenceJoystickConnectionWarning(true); // CameraServer.startAutomaticCapture(); @@ -688,7 +668,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 +719,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, 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 @@ -914,11 +894,19 @@ public class RobotContainer { // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } + public boolean autoChooserUpdated = false; public void makeAutoChooser() { autoChooser = new SendableChooser(); - File dir = new File("/home/lvuser/deploy/pathplanner/autos/"); - // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); + File dir; + + if(RobotBase.isReal()) { + dir = new File("/home/lvuser/deploy/pathplanner/autos/"); + } else { + // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); + dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos"); + } + String[] autos = dir.list(); if(autos == null) return; @@ -930,6 +918,7 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { + autoChooserUpdated = true; if (filename.equals("Taxi")) { autoCommand = new SequentialCommandGroup( new MoveForTimeCommand(m_robotSwerveDrive, @@ -966,12 +955,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..01cdb68 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,22 +10,31 @@ 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.SimConstants; +import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.elevator.ElevatorIO; +import frc4388.robot.subsystems.elevator.ElevatorReal; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.lidar.LidarIO; +import frc4388.robot.subsystems.lidar.LidarReal; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO; +import frc4388.robot.subsystems.swerve.SwerveReal; +import frc4388.robot.subsystems.vision.VisionIO; +import frc4388.robot.subsystems.vision.VisionReal; +import frc4388.utility.status.FaultCANCoder; +import frc4388.utility.status.FaultPhotonCamera; +import frc4388.utility.status.FaultPidgeon2; +import frc4388.utility.status.FaultTalonFX; /** * Defines and holds all I/O objects on the Roborio. This is useful for unit @@ -35,34 +44,132 @@ 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 RobotMap() { - configureDriveMotorControllers(); - } + public final VisionIO leftCamera; + public final VisionIO rightCamera; + + // public final LiDAR lidar = new + + public final LidarIO reefLidar; + public final LidarIO reverseLidar; + /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); /* Swreve Drive Subsystem */ - public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - Constants.SwerveDriveConstants.DrivetrainConstants, - Constants.SwerveDriveConstants.FRONT_LEFT, Constants.SwerveDriveConstants.FRONT_RIGHT, - Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT - ); + public final SwerveIO swerveDrivetrain; /* Elevator Subsystem */ - public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); - + public final ElevatorIO elevatorIO; - public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); - public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); - public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + public RobotMap(SimConstants.Mode mode) { + switch (mode) { + case REAL: + // Configure cameras + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - void configureDriveMotorControllers() { - // endeffector.saf + leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + + // Configure LiDAR + reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + + // Configure swerve drive train + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); + + swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + + // Configure elevator + + TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); + TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + + + DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); + DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); + DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); + + elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); + + + + // Fault + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + + FaultTalonFX.addDevice(elevator, "Elevator"); + FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + + break; + // case SIM: + // break; + default: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + reefLidar = new LidarIO() {}; + reverseLidar = new LidarIO() {}; + swerveDrivetrain = new SwerveIO() {}; + elevatorIO = new ElevatorIO() {}; + break; + } } + + // 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..3a4e043 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -4,8 +4,7 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.TimesNegativeOne; +import frc4388.robot.subsystems.swerve.SwerveDrive; // 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..c5b5e53 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; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class MoveUntilSuply extends Command { diff --git a/src/main/java/frc4388/robot/commands/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 69% rename from src/main/java/frc4388/robot/commands/GotoLastApril.java rename to src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 0686440..6a11e85 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.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.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Vision; +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 74% rename from src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java rename to src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index cda04a7..648a7d8 100644 --- a/src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -1,29 +1,24 @@ -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; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { private final SwerveDrive swerveDrive; private final Translation2d leftStick; private final Translation2d rightStick; - private final Lidar m_lidar; + private final LiDAR m_lidar; private final double mindistance; - private final boolean robotRelative; public DriveUntilLiDAR( SwerveDrive swerveDrive, Translation2d leftStick, Translation2d rightStick, - Lidar lidar, - double mindistance, - boolean robotRelative) { + LiDAR lidar, + 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 91% rename from src/main/java/frc4388/robot/commands/LidarAlign.java rename to src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index 8da22a3..19efd85 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -2,19 +2,19 @@ // 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; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Lidar; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LidarAlign extends Command { private SwerveDrive swerveDrive; - private Lidar lidar; + private LiDAR lidar; private int currentFinderTick; // private int tickFoundPipe; @@ -26,7 +26,7 @@ public class LidarAlign extends Command { // private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ - public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) { + public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) { // Use addRequirements() here to declare subsystem dependencies. this.swerveDrive = swerveDrive; @@ -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 90% rename from src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java rename to src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java index a2945c0..50e616c 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java @@ -2,11 +2,11 @@ // 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; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class RotateToAngle extends PID { diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java 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..bff5105 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; @@ -6,11 +6,11 @@ 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.robot.subsystems.swerve.SwerveDrive; +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 95% rename from src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java rename to src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java index 7f48a6c..4cf3159 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; @@ -7,11 +7,11 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.DataUtils; -import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.robot.subsystems.swerve.SwerveDrive; +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 93% rename from src/main/java/frc4388/robot/commands/waitElevatorRefrence.java rename to src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java index f3bd0fc..7d2ec77 100644 --- a/src/main/java/frc4388/robot/commands/waitElevatorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java @@ -2,10 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitElevatorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java similarity index 93% rename from src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java rename to src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java index 19fe2a8..1ff48cd 100644 --- a/src/main/java/frc4388/robot/commands/waitEndefectorRefrence.java +++ b/src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java @@ -2,10 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitEndefectorRefrence extends Command { diff --git a/src/main/java/frc4388/robot/commands/waitFeedCoral.java b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java similarity index 92% rename from src/main/java/frc4388/robot/commands/waitFeedCoral.java rename to src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java index e1e32f6..992879a 100644 --- a/src/main/java/frc4388/robot/commands/waitFeedCoral.java +++ b/src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java @@ -2,10 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.commands; +package frc4388.robot.commands.wait; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.Elevator; +import frc4388.robot.subsystems.elevator.Elevator; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class waitFeedCoral extends Command { diff --git a/src/main/java/frc4388/robot/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..5aeda6b --- /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 = 173; + public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513"; + public static final String GIT_DATE = "2025-07-17 09:15:19 MDT"; + public static final String GIT_BRANCH = "advantagekit"; + public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT"; + public static final long BUILD_UNIX_TIME = 1752774931371L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java new file mode 100644 index 0000000..6e8733d --- /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.compute.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/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index a95c0a7..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,108 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; - -/** - * Add your docs here. - */ -public class DiffDrive extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private TalonFX m_leftFrontMotor; - private TalonFX m_rightFrontMotor; - private TalonFX m_leftBackMotor; - private TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, - TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - super(); - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false)); - m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false)); - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - //SmartDashboard.putData(m_gyro); - } - - - @Override - public String getSubsystemName() { - return "Diff Drive"; - } - - @Override - public void queryStatus() { - // TODO: Add Stuff - } - - @Override - public Status diagnosticStatus() { - Log("Diagnostic info for this has not been inplemented!"); //TODO - return new Status(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java deleted file mode 100644 index cbd10bb..0000000 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ /dev/null @@ -1,402 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package 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; - -public class Elevator extends Subsystem { - /** Creates a new Elevator. */ - private TalonFX elevatorMotor; - private TalonFX endeffectorMotor; - private LED led; - - private long wait = 0; - private long maxWait = 1000; - - private double elevatorRefrence = 0; - private double endeffectorRefrence = 0; - - private boolean elevatorManualStop = true; - private boolean endefectorManualStop = true; - - private boolean disableAutoIntake = false; - - private boolean seededZeroEndefector = false; - private boolean seededZeroElevator = false; - - private DigitalInput basinBeamBreak; - private DigitalInput endeffectorLimitSwitch; - private DigitalInput intakeIR; - - public enum CoordinationState { - Waiting, // for coral into the though - WatingBeamTripped, //once the beam break trips - Ready, // Has coral in endefector - Hovering, // Has coral in endefector - L2Score, - L2ScoreLeave, - PrimedThree, // Arm and elevator Waiting to score in the level 3 position - ScoringThree, // Arm and elevator in the level three position - PrimedFour, // Arm and elevator Waiting to score in the level 4 position - ScoringFour, // Arm and elevator in the level four position - BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef. - BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef. - BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef. - BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef. - } - - private CoordinationState currentState; - - // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { - public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) { - elevatorMotor = elevatorTalonFX; - endeffectorMotor = endeffectorTalonFX; - this.led = led; - - this.basinBeamBreak = basinLimitSwitch; - this.endeffectorLimitSwitch = endeffectorLimitSwitch; - this.intakeIR = intakeDigitalInput; - - elevatorMotor.setNeutralMode(NeutralModeValue.Brake); - endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); - - elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); - endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); - currentState = CoordinationState.Ready; - } - - //PID methods - - private void PIDPosition(TalonFX motor, double position) { - if (motor == elevatorMotor) elevatorRefrence = position; - else endeffectorRefrence = position; - - var request = new PositionDutyCycle(position); - motor.setControl(request); - } - - public void elevatorStop() { - elevatorMotor.set(0); - } - - public void endeffectorStop() { - endeffectorMotor.set(0); - } - - - public void transitionState(CoordinationState state) { - // elevatorMotor.enable(); - - - currentState = state; - switch (currentState) { - case Waiting: { - wait = System.currentTimeMillis() + maxWait; - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); - led.setMode(LEDConstants.WAITING_PATTERN); - break; - } - - case WatingBeamTripped: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); - led.setMode(LEDConstants.DOWN_PATTERN); - break; - } - - case Ready: { - PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); - led.setMode(LEDConstants.DOWN_PATTERN); - break; - } - - case Hovering: { - PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); - led.setMode(LEDConstants.READY_PATTERN); - break; - } - - case L2Score: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case L2ScoreLeave: { - PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case PrimedFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case ScoringFour: { - PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case PrimedThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - case BallRemoverL3Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); - break; - } - - case BallRemoverL3Go: { - PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(LEDConstants.SCORING_PATTERN); - break; - } - - default: { - assert false; - } - } - - } - - public void togggleAutoIntaking() { - disableAutoIntake = !disableAutoIntake; - } - - public boolean elevatorAtReference() { - // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble(); - double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); - double diffrence = elevatorRefrence - elevatorPosition; - - boolean headedUp = diffrence < 0; - boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; - - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); - } - - public boolean endeffectorAtReference() { - // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble(); - double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); - double diffrence = endeffectorRefrence - endeffectorPosition; - - boolean headedUp = diffrence < 0; - boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; - boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; - - return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp)); - } - // public void driveElevatorStick(Translation2d stick) { - // if (stick.getNorm() > 0.05) { - // elevatorMotor.set(stick.getY()); - // } - // } - - public boolean getEndeffectorLimit() { - return endeffectorLimitSwitch.get(); - } - - private void periodicWaiting() { - if (!basinBeamBreak.get()) - transitionState(CoordinationState.Ready); - // if(!endeffectorLimitSwitch.get()) - // transitionState(CoordinationState.Hovering); - } - - // private void periodicWaitingTripped() { - // if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) - // transitionState(CoordinationState.Ready); - // } - - private void periodicReady() { - if (elevatorAtReference() && !endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Hovering); - if(elevatorAtReference() && endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Hovering); - } - - private void periodicScoring() { - if (!endeffectorLimitSwitch.get()) - transitionState(CoordinationState.Waiting); - } - - public void manualElevatorVel(double velocity) { - if (Math.abs(velocity) > 0.1) { - elevatorMotor.set(velocity); - elevatorManualStop = false; - return; - } - if (!elevatorManualStop) { - elevatorManualStop = true; - elevatorMotor.set(0); - } - } - - public void manualEndeffectorVel(double velocity) { - if (Math.abs(velocity) > 0.1) { - endeffectorMotor.set(velocity); - endefectorManualStop = false; - return; - } - if (!endefectorManualStop) { - endefectorManualStop = true; - endeffectorMotor.set(0); - } - } - - @Override - public void periodic() { - - // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); - // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); - // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); - // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); - - - // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ - // PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); - // } - - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); - // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); - SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); - SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); - SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); - SmartDashboard.putString("State", currentState.toString()); - - if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) { - endeffectorMotor.setPosition(0); - seededZeroEndefector = !seededZeroEndefector; - } - - if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) { - elevatorMotor.setPosition(0); - seededZeroElevator = !seededZeroElevator; - } - - if (disableAutoIntake) return; - - if (currentState == CoordinationState.Waiting) { - periodicWaiting(); - } else if (currentState == CoordinationState.WatingBeamTripped) { - // periodicWaitingTripped(); - } else if (currentState == CoordinationState.Ready) { - periodicReady(); - } - - if(!intakeIR.get()){ - led.setMode(LEDConstants.DOWN_PATTERN); - } - - - // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { - // periodicScoring(); - // } - } - - public boolean isL4Primed() { - return currentState == CoordinationState.PrimedFour; - } - - public boolean isL3Primed() { - return currentState == CoordinationState.PrimedThree; - } - - public boolean hasCoral() { - return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get(); - } - - public boolean readyToMove() { - return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get(); - // return hasCoral(); - } - - public void armShuffle(){ - if(!basinBeamBreak.get()){ - //shuffle the coral with the arm until coral hits beam break - } - } - - @Override - public String getSubsystemName() { - return "Elevator"; - } - - @Override - public void queryStatus() {} - - @Override - public Status diagnosticStatus() { - Status status = new Status(); - - status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name()); - status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor); - status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor); - - return status; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e85100f..c050278 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,24 +7,26 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.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.FaultReporter; +import frc4388.utility.status.Queryable; +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 * Driver */ -public class LED extends Subsystem { +public class LED extends SubsystemBase implements Queryable { + public LED() { + FaultReporter.register(this); + } private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; @@ -47,23 +49,21 @@ public class LED extends Subsystem { LEDController.set(mode.getValue()); } - @Override - public String getSubsystemName() { - return "LEDs"; + @AutoLogOutput + public String state() { + return mode.getClass().toString(); } @Override - public void queryStatus() { - SmartDashboard.putString("LED status", mode.name()); + public String getName() { + return "LEDs"; } @Override public Status diagnosticStatus() { Status status = new Status(); - if(LEDController.isAlive()) - status.addReport(ReportLevel.INFO, "LED is CONNECTED"); - else + if(!LEDController.isAlive()) status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name()); diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java deleted file mode 100644 index e660301..0000000 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ /dev/null @@ -1,92 +0,0 @@ -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; - -// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface -public class Lidar extends Subsystem { - - private Counter LidarPWM; - private String name = "Lidar"; - - private double distance = -1; - public Lidar(int port, String name) { - this.name = name; - LidarPWM = new Counter(port); - LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured - LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement - LidarPWM.reset(); - - - subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - sbDistance = subsystemLayout - .add("Distance", 0) - .withWidget(BuiltInWidgets.kGraph) - .getEntry(); - - sbWithinDistance = subsystemLayout - . add("Within Distance", 0) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - } - - @Override - public void periodic() { - if(LidarPWM.get() < 1) - distance = -1; - else - distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; - } - - public double getDistance(){ - return distance; - } - - public boolean withinDistance(){ - if(distance == -1) return false; - return distance < LiDARConstants.LIDAR_DETECT_DISTANCE; - } - - ShuffleboardLayout subsystemLayout; - GenericEntry sbDistance; - GenericEntry sbWithinDistance; - - @Override - public String getSubsystemName() { - return "Lidar " + name; - } - - @Override - public void queryStatus() { - sbDistance.setDouble(distance); - sbWithinDistance.setBoolean(withinDistance()); - } - - @Override - public Status diagnosticStatus() { - Status s = new Status(); - - if(distance == -1){ - s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); - }else{ - s.addReport(ReportLevel.INFO, "LiDAR Connected"); - } - - s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance); - - return s; - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 4f34193..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,385 +0,0 @@ -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; - -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; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; - -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.Constants.FieldConstants; -import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.Status; -import frc4388.utility.Subsystem; -import frc4388.utility.Status.ReportLevel; - -public class Vision extends Subsystem { - - // private PhotonCamera leftCamera; - // private PhotonCamera rightCamera; - - private PhotonCamera[] cameras; - private PhotonPoseEstimator[] estimators; - private List poses = new ArrayList<>(); - - private boolean isTagDetected = false; - private boolean isTagProcessed = false; - - private double lastLatency = 0; - - public double getLastLatency() { - return lastLatency; - } - - public Pose2d lastVisionPose = new Pose2d(); - private Pose2d lastPhysOdomPose = new Pose2d(); - - private Matrix curStdDevs; - - private Field2d field = new Field2d(); - - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbTagDetected = subsystemLayout - .add("Tag Detected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbTagProcessed = subsystemLayout - .add("Tag Processed", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbLeftCamConnected = subsystemLayout - .add("Left Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbRightCamConnected = subsystemLayout - .add("Right Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ - SmartDashboard.putData(field); - - this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; - - PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); - PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); - - photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; - // 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 void update() { - isTagProcessed = false; - isTagDetected = false; - - Instant now = Instant.now(); - - int cams = 0; - - // double X = 0; - // double Y = 0; - // double Yaw = 0; - double latency = 0; - - // Pose2d pose = null; - poses.clear(); - - for(int i = 0; i < cameras.length; i++){ - PhotonCamera camera = cameras[i]; - PhotonPoseEstimator estimator = estimators[i]; - - var results = camera.getAllUnreadResults(); - - // If there are no more updates from the camera - if (results.size() == 0) - continue; - - - var result = results.get(results.size()-1); - latency += result.getTimestampSeconds(); - - isTagDetected = isTagDetected | result.hasTargets(); - - // If there are no tags - if(!result.hasTargets()) - continue; - - Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); - - // If the tag was failed to be processed - if(estimatedRobotPose.isEmpty()) - continue; - - poses.add(estimatedRobotPose.get()); - - // if(pose == null) - // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - // else - // pose = pose.interpolate(pose, 0.5); - // X += pose.getX(); - // Y += pose.getY(); - - // if(X > 6) - - // Yaw += (pose.getRotation().getDegrees() + 180) % 360; - // cams++; - - isTagProcessed = true; - - - } - - // 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; - // } - } - - - /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should - * only be called once per loop. - * - *

Also includes updates for the standard deviations, which can (optionally) be retrieved with - * {@link getEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets - * used for estimation. - */ - public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { - Optional visionEst = Optional.empty(); - - var targets = change.getTargets(); - for(int i = targets.size()-1; i >= 0; i--){ - Transform3d pos = targets.get(i).getBestCameraToTarget(); - double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); - if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { - change.targets.remove(i); - } - } - - if(targets.size() <= 0) - return visionEst; // Will be empty - - visionEst = estimator.update(change); - // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); - - return visionEst; - } - - - // /** - // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard - // * deviations based on number of tags, estimation strategy, and distance from the tags. - // * - // * @param estimatedPose The estimated pose to guess standard deviations for. - // * @param targets All targets in this camera frame - // */ - // private void updateEstimationStdDevs( - // Optional estimatedPose, - // List targets, - // PhotonPoseEstimator estimator) { - // if (estimatedPose.isEmpty()) { - // // No pose input. Default to single-tag std devs - // curStdDevs = VisionConstants.kSingleTagStdDevs; - - // } else { - // // Pose present. Start running Heuristic - // var estStdDevs = VisionConstants.kSingleTagStdDevs; - // int numTags = 0; - // double avgDist = 0; - - // // Precalculation - see how many tags we found, and calculate an average-distance metric - // for (var tgt : targets) { - // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - // if (tagPose.isEmpty()) continue; - - // double distance = tagPose - // .get() - // .toPose2d() - // .getTranslation() - // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - - // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - // numTags++; - // avgDist += distance; - // } - // } - - // if (numTags == 0) { - // // No tags visible. Default to single-tag std devs - // curStdDevs = VisionConstants.kSingleTagStdDevs; - // } else { - // // One or more tags visible, run the full heuristic. - // avgDist /= numTags; - // // Decrease std devs if multiple targets are visible - // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; - // // Increase std devs based on (average) distance - // if (numTags == 1 && avgDist > 4) - // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - // curStdDevs = estStdDevs; - // } - // } - // } - - /** - * Returns the latest standard deviations of the estimated pose from {@link - * #getEstimatedGlobalPose()}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. - */ - public Matrix getEstimationStdDevs() { - return curStdDevs; - } - - - - - public void setLastOdomPose(Optional pose){ - if(pose.isPresent()) - lastPhysOdomPose = pose.get(); - } - - // public double getLastOdomSpeed(){ - // return lastOdomSpeed; - // } - - public Pose2d getPose2d() { - if(lastPhysOdomPose != null) - return lastPhysOdomPose; - return new Pose2d(); - // if(isTagDetected && isTagProcessed) - // // return lastVisionPose; - // else - // return lastPhysOdomPose; - } - - public static double getTime() { - return Utils.getCurrentTimeSeconds(); - } - - public boolean isTag(){ - return isTagDetected && isTagProcessed; - } - - - public void addVisionMeasurement( SwerveDrivetrain drivetrain){ - for(EstimatedRobotPose pose : poses){ - drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); - } - } - - - - - - - - - @Override - public String getSubsystemName() { - return "Vision"; - } - -// GenericEntry sbShiftState = subsystemLayout -// .add("Shift State", 0) -// .withWidget(BuiltInWidgets.kNumberBar) -// .getEntry(); - - - @Override - public void queryStatus() { - sbTagDetected.setBoolean(isTagDetected); - sbTagProcessed.setBoolean(isTagProcessed); - sbLeftCamConnected.setBoolean(cameras[0].isConnected()); - sbRightCamConnected.setBoolean(cameras[1].isConnected()); - // field.setRobotPose(getPose2d()); - } - - @Override - public Status diagnosticStatus() { - Status status = new Status(); - - if(cameras[0].isConnected()) - status.addReport(ReportLevel.INFO, "Left Camera Connected"); - else - status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED"); - - if(cameras[1].isConnected()) - status.addReport(ReportLevel.INFO, "Right Camera Connected"); - else - status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED"); - - - return status; - } - -} diff --git a/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..a054425 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,379 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.ElevatorConstants; +import frc4388.robot.constants.Constants.LEDConstants; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; + +public class Elevator extends SubsystemBase implements Queryable { + ElevatorIO io; + ElevatorStateAutoLogged state = new ElevatorStateAutoLogged(); + + /** Creates a new Elevator. */ + private LED led; + + @SuppressWarnings("unused") + public long wait = 0; + public long maxWait = 1000; + + public boolean elevatorManualStop = true; + public boolean endefectorManualStop = true; + + public boolean disableAutoIntake = false; + + public boolean seededZeroEndefector = false; + public boolean seededZeroElevator = false; + + // private ElevatorState state = new ElevatorState(); + + public enum CoordinationState { + Waiting, // for coral into the though + WatingBeamTripped, //once the beam break trips + Ready, // Has coral in endefector + Hovering, // Has coral in endefector + L2Score, + L2ScoreLeave, + PrimedThree, // Arm and elevator Waiting to score in the level 3 position + ScoringThree, // Arm and elevator in the level three position + PrimedFour, // Arm and elevator Waiting to score in the level 4 position + ScoringFour, // Arm and elevator in the level four position + BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef. + BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef. + BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef. + BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef. + } + + private CoordinationState currentState; + + // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) { + public Elevator(ElevatorIO io, LED led) { + this.io = io; + this.led = led; + + currentState = CoordinationState.Ready; + + FaultReporter.register(this); + } + + + public void transitionState(CoordinationState state) { + // elevatorMotor.enable(); + + + currentState = state; + switch (currentState) { + case Waiting: { + wait = System.currentTimeMillis() + maxWait; + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0)); + led.setMode(LEDConstants.WAITING_PATTERN); + break; + } + + case WatingBeamTripped: { + io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); + break; + } + + case Ready: { + io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0)); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.DOWN_PATTERN); + break; + } + + case Hovering: { + io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR); + led.setMode(LEDConstants.READY_PATTERN); + break; + } + + case L2Score: { + io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case L2ScoreLeave: { + io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case PrimedFour: { + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case ScoringFour: { + io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case PrimedThree: { + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case ScoringThree: { + io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL2Primed: { + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL2Go: { + io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + case BallRemoverL3Primed: { + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + break; + } + + case BallRemoverL3Go: { + io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); + led.setMode(LEDConstants.SCORING_PATTERN); + break; + } + + default: { + assert false; + } + } + + } + + public void togggleAutoIntaking() { + disableAutoIntake = !disableAutoIntake; + } + + public boolean elevatorAtReference() { + double diffrence = state.elevatorRefrence - state.elevatorPosition; + + boolean headedUp = diffrence < 0; + + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.elevatorForwardLimit && !headedUp) + ); + } + + public boolean endeffectorAtReference() { + double diffrence = state.endeffectorRefrence - state.endeffectorPosition; + + boolean headedUp = diffrence < 0; + + return (Math.abs(diffrence) <= 0.5 + || (state.elevatorReverseLimit && headedUp) + || (state.endeffectorForwardLimit && !headedUp) + ); + } + // public void driveElevatorStick(Translation2d stick) { + // if (stick.getNorm() > 0.05) { + // elevatorMotor.set(stick.getY()); + // } + // } + + public boolean getEndeffectorLimit() { + return state.endeffectorLimitSwitch; + } + + private void periodicWaiting() { + if (!state.basinBeamBreak) + transitionState(CoordinationState.Ready); + // if(!endeffectorLimitSwitch.get()) + // transitionState(CoordinationState.Hovering); + } + + // private void periodicWaitingTripped() { + // if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) + // transitionState(CoordinationState.Ready); + // } + + private void periodicReady() { + if (elevatorAtReference() && !state.endeffectorLimitSwitch) + transitionState(CoordinationState.Hovering); + if(elevatorAtReference() && state.endeffectorLimitSwitch) + transitionState(CoordinationState.Hovering); + } + + @SuppressWarnings("unused") + private void periodicScoring() { + if (!state.endeffectorLimitSwitch) + transitionState(CoordinationState.Waiting); + } + + public void manualElevatorVel(double velocity) { + if (Math.abs(velocity) > 0.1) { + io.elevatorToVelocity(velocity); + elevatorManualStop = false; + return; + } + if (!elevatorManualStop) { + elevatorManualStop = true; + io.elevatorToVelocity(0); + } + } + + public void manualEndeffectorVel(double velocity) { + if (Math.abs(velocity) > 0.1) { + io.endeffectorToVelocity(velocity); + endefectorManualStop = false; + return; + } + if (!endefectorManualStop) { + endefectorManualStop = true; + io.endeffectorToVelocity(0); + } + } + + @Override + public void periodic() { + + // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble(); + // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble(); + // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble(); + // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble(); + + + // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){ + // PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble()); + // } + + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity); + // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque); + // SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0); + // SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0); + // SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0); + // SmartDashboard.putString("State", currentState.toString()); + + io.updateInputs(state); + Logger.processInputs("Elevator", state); + + if (!seededZeroEndefector && state.endeffectorForwardLimit) { + io.endeffectorToPosition(0); + seededZeroEndefector = !seededZeroEndefector; + } + + if (!seededZeroElevator && state.elevatorReverseLimit) { + io.endeffectorToPosition(0); + seededZeroElevator = !seededZeroElevator; + } + + if (disableAutoIntake) return; + + if (currentState == CoordinationState.Waiting) { + periodicWaiting(); + } else if (currentState == CoordinationState.WatingBeamTripped) { + // periodicWaitingTripped(); + } else if (currentState == CoordinationState.Ready) { + periodicReady(); + } + + if(!state.intakeIR){ + led.setMode(LEDConstants.DOWN_PATTERN); + } + + + // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { + // periodicScoring(); + // } + } + + @AutoLogOutput(key="Elevator/state") + public String getState() { + return currentState.toString(); + } + + public boolean isL4Primed() { + return currentState == CoordinationState.PrimedFour; + } + + public boolean isL3Primed() { + return currentState == CoordinationState.PrimedThree; + } + + public boolean hasCoral() { + return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch; + } + + public void elevatorStop() { + io.elevatorToVelocity(0); + } + + public void endeffectorStop() { + io.endeffectorToVelocity(0); + } + + public boolean readyToMove() { + return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch; + // return hasCoral(); + } + + public void armShuffle(){ + if(!state.basinBeamBreak){ + //shuffle the coral with the arm until coral hits beam break + } + } + + @Override + public String getName() { + return "Elevator"; + } + + // @Override + // public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name()); + + return status; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..a48278c --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,34 @@ +package frc4388.robot.subsystems.elevator; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.subsystems.lidar.LidarIO.LidarState; + +public interface ElevatorIO { + @AutoLog + public class ElevatorState { + public double elevatorRefrence; + public double elevatorPosition; + public boolean elevatorForwardLimit; + public boolean elevatorReverseLimit; + + public double endeffectorRefrence; + public double endeffectorPosition; + public boolean endeffectorForwardLimit; + public boolean endeffectorReverseLimit; + + + public boolean basinBeamBreak; + public boolean endeffectorLimitSwitch; + public boolean intakeIR; + + } + + public default void elevatorToPosition(double position) {} + public default void endeffectorToPosition(double position) {} + public default void elevatorToVelocity(double velocity) {} + public default void endeffectorToVelocity(double velocity) {} + + public default void updateInputs(ElevatorState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java new file mode 100644 index 0000000..89d48aa --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.elevator; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.DigitalInput; +import frc4388.robot.constants.Constants.ElevatorConstants; + +public class ElevatorReal implements ElevatorIO { + TalonFX elevatorMotor; + TalonFX endeffectorMotor; + + DigitalInput basinLimitSwitch; + DigitalInput endeffectorLimitSwitch; + DigitalInput intakeIR; + + + double elevatorRefrence = 0; + double endeffectorRefrence = 0; + + public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) { + this.elevatorMotor = elevatorTalonFX; + this.endeffectorMotor = endeffectorTalonFX; + + this.basinLimitSwitch = basinLimitSwitch; + this.endeffectorLimitSwitch = endeffectorLimitSwitch; + this.intakeIR = intakeIR; + + + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); + endeffectorMotor.setNeutralMode(NeutralModeValue.Brake); + + elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID); + endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID); + } + + @Override + public void updateInputs(ElevatorState state) { + state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble(); + state.elevatorRefrence = elevatorRefrence; + state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0; + state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0; + + state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble(); + state.endeffectorRefrence = endeffectorRefrence; + state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0; + state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0; + + + state.basinBeamBreak = basinLimitSwitch.get(); + state.endeffectorLimitSwitch = endeffectorLimitSwitch.get(); + state.intakeIR = intakeIR.get(); + } + + @Override + public void elevatorToPosition(double position) { + elevatorRefrence = position; + var request = new PositionDutyCycle(position); + elevatorMotor.setControl(request); + } + + @Override + public void endeffectorToPosition(double position) { + endeffectorRefrence = position; + var request = new PositionDutyCycle(position); + endeffectorMotor.setControl(request); + } + + @Override + public void elevatorToVelocity(double velocity) { + elevatorMotor.set(velocity); + } + + + @Override + public void endeffectorToVelocity(double velocity) { + endeffectorMotor.set(velocity); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java new file mode 100644 index 0000000..2945c2d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -0,0 +1,58 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status.ReportLevel; + +public class LiDAR extends SubsystemBase implements Queryable { + LidarIO io; + LidarStateAutoLogged state = new LidarStateAutoLogged(); + + private String name = "Lidar"; + + public LiDAR(LidarIO device, String name) { + FaultReporter.register(this); + + this.io = device; + this.name = name; + } + + @Override + public void periodic() { + io.updateInputs(state); + Logger.processInputs("LiDAR/"+name, state); + } + + // @AutoLogOutput(key = "Lidar/{name}") + public double getDistance(){ + return state.distance; + } + + public boolean withinDistance(){ + if(state.distance == -1) return false; + return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE; + } + + @Override + public String getName() { + return "Lidar " + name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(state.distance == -1) + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance); + + return s; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java new file mode 100644 index 0000000..e3b4ebc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java @@ -0,0 +1,13 @@ +package frc4388.robot.subsystems.lidar; + +import org.littletonrobotics.junction.AutoLog; + +public interface LidarIO { + @AutoLog + public class LidarState { + public boolean connected; + public double distance; + } + + public default void updateInputs(LidarState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java new file mode 100644 index 0000000..3bf33d5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems.lidar; + +import edu.wpi.first.wpilibj.Counter; +import frc4388.robot.constants.Constants.LiDARConstants; + +// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface +public class LidarReal implements LidarIO { + + + private Counter LidarPWM; + + public LidarReal(int port) { + LidarPWM = new Counter(port); + LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured + LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement + LidarPWM.reset(); + } + + @Override + public void updateInputs(LidarState state) { + + if(LidarPWM.get() < 1) + state.distance = -1; + else + state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java similarity index 69% rename from src/main/java/frc4388/robot/subsystems/SwerveDrive.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index c1f9335..bbc61e4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -2,65 +2,53 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.swerve; -import java.util.Optional; -import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; -import 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 edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.status.Status; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; 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 { - private SwerveDrivetrain swerveDriveTrain; +public class SwerveDrive extends SubsystemBase implements Queryable { + // private SwerveDrivetrain swerveDriveTrain; + + private SwerveIO io; + private SwerveStateAutoLogged state; private Vision vision; - private int gear_index = SwerveDriveConstants.STARTING_GEAR; - private boolean stopped = false; - private boolean robotKnowsWhereItIs = false; + + + public int gear_index = SwerveDriveConstants.STARTING_GEAR; + public boolean stopped = false; + public 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 - // 25% + // 25% public double lastOdomSpeed; - public Pose2d initalPose2d = null; + public Pose2d initalPose2d = new Pose2d(); public double rotTarget = 0.0; @@ -68,12 +56,14 @@ public class SwerveDrive extends Subsystem { public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain // swerveDriveTrain) { - super(); + FaultReporter.register(this); + + this.io = swerveDriveTrain; + this.state = new SwerveStateAutoLogged(); - this.swerveDriveTrain = swerveDriveTrain; this.vision = vision; RobotConfig config; @@ -86,12 +76,12 @@ public class SwerveDrive extends Subsystem { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + return getPose2d(); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for @@ -115,12 +105,36 @@ 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", toString())), + // new SysIdRoutine.Mechanism( + // (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + } public void setOdoPose(Pose2d pose) { if (pose == null) return; initalPose2d = pose; - swerveDriveTrain.resetPose(pose); + io.resetPose(pose); } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -154,11 +168,14 @@ public class SwerveDrive extends Subsystem { // ! drift correction if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { - rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + + rotTarget = state.currentPose.getRotation().getDegrees(); + + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { @@ -171,13 +188,13 @@ public class SwerveDrive extends Subsystem { SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); SmartDashboard.putBoolean("drift correction", true); } } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + io.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(-leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); @@ -188,7 +205,7 @@ public class SwerveDrive extends Subsystem { stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + io.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); @@ -210,7 +227,7 @@ public class SwerveDrive extends Subsystem { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); @@ -228,7 +245,7 @@ public class SwerveDrive extends Subsystem { SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { @@ -239,35 +256,23 @@ 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); - } - - public void setLimits(double limitInAmps) { - for (SwerveModule module : swerveDriveTrain.getModules()) { - var talonFXConfigurator = module.getDriveMotor().getConfigurator(); - var talonFXConfigs = new TalonFXConfiguration(); - - talonFXConfigurator.refresh(talonFXConfigs); - talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; - talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; - talonFXConfigurator.apply(talonFXConfigs); - } + io.setControl(ctrl); } public void activateLuigiMode() { - setLimits(20); + io.setLimits(20); } public void deactivateLuigiMode() { - setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); } public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) .withVelocityY(0) .withTargetDirection(Rotation2d.fromDegrees(angle))); @@ -279,6 +284,10 @@ public class SwerveDrive extends Subsystem { return false; } + public boolean isStopped() { + return lastOdomSpeed < AutoConstants.STOP_VELOCITY; + } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the // swerve drive is still going: @@ -289,7 +298,7 @@ public class SwerveDrive extends Subsystem { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); @@ -301,11 +310,13 @@ public class SwerveDrive extends Subsystem { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + if(state.currentPose == null) + return initalPose2d; + return state.currentPose; } public void resetGyro() { - swerveDriveTrain.tareEverything(); + io.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; // vision.resetRotations(); @@ -314,7 +325,7 @@ public class SwerveDrive extends Subsystem { public void softStop() { stopped = true; - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) .withRotationalRate(0) @@ -333,27 +344,21 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); SmartDashboard.putNumber("RotTartget", rotTarget); - double time = Vision.getTime(); - double freq = swerveDriveTrain.getOdometryFrequency(); - - Optional curpose = swerveDriveTrain.samplePoseAt(time); - Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); - vision.setLastOdomPose(curpose); - setLastOdomSpeed(curpose, lastPose, freq); + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { robotKnowsWhereItIs = true; - Pose2d currPose = getPose2d(); - double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees(); - rotTarget += deltaAngle; + Pose2d curPose = getPose2d(); + rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); } - - vision.addVisionMeasurement(swerveDriveTrain); - // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); - //back to the ~~future~~ *past* + + io.addVisionMeasurement(vision.getPosesToAdd()); } // if(e.isPresent()) @@ -431,57 +436,32 @@ public class SwerveDrive extends Subsystem { - public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - 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); + public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){ + if(curPose != null && lastPose != null){ + lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; } } - - @Override - public String getSubsystemName() { - return "Swerve Drive Controller"; + @AutoLogOutput(key="SwerveDrive/speed ") + public double getOdometrySpeed() { + return lastOdomSpeed; } + - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbGyro = subsystemLayout - .add("Gyro angle", 0) - .withWidget(BuiltInWidgets.kGyro) - .getEntry(); - - GenericEntry sbShiftState = subsystemLayout - .add("Shift State", 0) - .withWidget(BuiltInWidgets.kNumberBar) - .getEntry(); @Override - public void queryStatus() { - sbGyro.setDouble(getGyroAngle()); - sbShiftState.setDouble(this.speedAdjust); - - // TODO: Add more status things + public String getName() { + return "Swerve Drive Controller"; } @Override public Status diagnosticStatus() { Status status = new Status(); - status.addReport(ReportLevel.INFO, - "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); + // status.addReport(ReportLevel.INFO, + // "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); return status; } } + diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java new file mode 100644 index 0000000..35b02db --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -0,0 +1,246 @@ +package frc4388.robot.subsystems.swerve; + +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 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(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/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..fa7de1a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,33 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public interface SwerveIO { + @AutoLog + public class SwerveState { + public Pose2d currentPose = null; + public Pose2d lastPose = null; + public ChassisSpeeds speeds = null; + public double odometryRate = 1; + } + + public default void setControl(SwerveRequest ctrl) {} + + public default void setLimits(double limitInAmps) {} + + public default void tareEverything() {} + + public default void resetPose(Pose2d pose) {} + + public default void addVisionMeasurement(List poses) {} + + public default void updateInputs(SwerveState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java new file mode 100644 index 0000000..05d9e8f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -0,0 +1,63 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import frc4388.robot.subsystems.vision.Vision; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SwerveReal implements SwerveIO { + SwerveDrivetrain swerveDriveTrain; + + public SwerveReal(SwerveDrivetrain swerveDriveTrain) { + this.swerveDriveTrain = swerveDriveTrain; + swerveDriveTrain.getOdometryFrequency(); + } + + @Override + public void updateInputs(SwerveState state) { + double time = Vision.getTime(); + state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency(); + state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null); + state.speeds = swerveDriveTrain.getState().Speeds; + } + + @Override + public void setControl(SwerveRequest ctrl) { + swerveDriveTrain.setControl(ctrl); + } + + @Override + public void tareEverything() { + swerveDriveTrain.tareEverything(); + } + + @Override + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + @Override + public void addVisionMeasurement(List poses) { + for(PoseObservation pose : poses) { + swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..a069af7 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -0,0 +1,95 @@ +package frc4388.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.FaultReporter; +import frc4388.utility.status.Queryable; +import frc4388.utility.status.Status; + +public class Vision extends SubsystemBase implements Queryable { + VisionIO[] io; + VisionStateAutoLogged[] state; + + + public Pose2d lastVisionPose = new Pose2d(); + public Pose2d lastPhysOdomPose = new Pose2d(); + + public Vision(VisionIO... devices) { + FaultReporter.register(this); + io = devices; + state = new VisionStateAutoLogged[io.length]; + + for(int i = 0; i < io.length; i++) { + state[i] = new VisionStateAutoLogged(); + } + } + + @Override + public void periodic() { + for(int i = 0; i < io.length; i++) { + io[i].updateInputs(state[i]); + Logger.processInputs("Vision/Camera" + i , state[i]); + } + } + + public List getPosesToAdd(){ + List poses = new ArrayList<>(); + for(int i = 0; i < state.length; i++) { + if(state[i].lastEstimatedPose != null) { + poses.add(state[i].lastEstimatedPose); + } + } + + return poses; + } + + public void setLastOdomPose(Pose2d pose){ + if(pose != null) + lastPhysOdomPose = pose; + } + + public boolean isTag(){ + for(int i = 0; i < state.length; i++){ + if(state[i].isTagDetected && state[i].isTagProcessed) + return true; + } + return false; + } + + @AutoLogOutput + public Pose2d getPose2d() { + if(lastPhysOdomPose != null) + return lastPhysOdomPose; + + // if(lastVisionPose != null) + // return lastVisionPose; + return new Pose2d(); + + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + @Override + public Status diagnosticStatus() { + return new Status(); + // // TODO Auto-generated method stub + // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..f979135 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,22 @@ +package frc4388.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisionIO { + @AutoLog + public class VisionState { + public boolean isTagDetected = false; + public boolean isTagProcessed = false; + // public double latency = 0; + public PoseObservation lastEstimatedPose = null; + } + + public static record PoseObservation( + Pose2d pose, + double timestamp + ) {} + + public default void updateInputs(VisionState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java new file mode 100644 index 0000000..1c12437 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -0,0 +1,158 @@ +package frc4388.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.Constants.VisionConstants; + +public class VisionReal implements VisionIO { + // private PhotonCamera[] cameras; + // private PhotonPoseEstimator[] estimators; + + private PhotonCamera camera; + private PhotonPoseEstimator estimator; + + // public List poses = new ArrayList<>(); + + + public VisionReal(PhotonCamera camera, Transform3d position){ + this.camera = camera; + estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position); + estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + // private Instant lastVisionTime = null; + + + public void updateInputs(VisionState state) { + state.isTagProcessed = false; + state.isTagDetected = false; + + state.lastEstimatedPose = null; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + return; + + + var result = results.get(results.size()-1); + + state.isTagDetected = state.isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + return; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + return; + + EstimatedRobotPose pose = estimatedRobotPose.get(); + + state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds); + + state.isTagProcessed = true; + } + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { + Optional visionEst = Optional.empty(); + + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); + } + } + + if(targets.size() <= 0) + return visionEst; // Will be empty + + visionEst = estimator.update(change); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + + return visionEst; + } + + public String getName() { + return camera.getName(); + } + + + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; + + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; + + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } + + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } +} diff --git a/src/main/java/frc4388/utility/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/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java index 20d3c57..c5a558e 100644 --- a/src/main/java/frc4388/utility/DeferredBlock.java +++ b/src/main/java/frc4388/utility/DeferredBlock.java @@ -2,22 +2,40 @@ package frc4388.utility; import java.util.ArrayList; +// Class for running code snippets whenever the robot is enabled. public class DeferredBlock { - private static ArrayList m_blocks = new ArrayList<>(); + private static ArrayList m_blocks_norerun = new ArrayList<>(); + private static ArrayList m_blocks_rerun = new ArrayList<>(); private static boolean m_hasRun = false; - public DeferredBlock(Runnable block) { - m_blocks.add(block); + public static void addBlock(Runnable block) { + addBlock(block, false); + } + + + public static void addBlock(Runnable block, boolean rerun) { + if(rerun) { + m_blocks_rerun.add(block); + } else { + m_blocks_norerun.add(block); + } } public static void execute() { - if (m_hasRun) return; - for (Runnable block : m_blocks) { + // Run blocks that run multiple times. + for (Runnable block : m_blocks_rerun) { block.run(); } - m_blocks.clear(); // for garbage collection + // Run blocks that only run once + if (m_hasRun) return; + + for (Runnable block : m_blocks_norerun) { + block.run(); + } + + m_blocks_norerun.clear(); // for garbage collection m_hasRun = true; } } diff --git a/src/main/java/frc4388/utility/DeferredBlockMulti.java b/src/main/java/frc4388/utility/DeferredBlockMulti.java deleted file mode 100644 index 9e8b284..0000000 --- a/src/main/java/frc4388/utility/DeferredBlockMulti.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc4388.utility; - -import java.util.ArrayList; - -public class DeferredBlockMulti { - private static ArrayList m_blocks = new ArrayList<>(); - - public DeferredBlockMulti(Runnable block) { - m_blocks.add(block); - } - - public static void execute() { - - for (Runnable block : m_blocks) { - block.run(); - } - } -} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java deleted file mode 100644 index 3ae503f..0000000 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ /dev/null @@ -1,269 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.utility; - -// import com.ctre.phoenix.sensors.WPI_Pigeon2; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; - -// import edu.wpi.first.wpilibj.GyroBase; -// import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * Gyro class that allows for interchangeable use between a pigeon and a navX - */ -public class RobotGyro { - private RobotTime m_robotTime = RobotTime.getInstance(); - - private Pigeon2 m_pigeon = null; - private AHRS m_navX = null; - public boolean m_isGyroAPigeon; //true if pigeon, false if navX - - private double m_lastPigeonAngle; - private double m_deltaPigeonAngle; - - private double pitchZero = 0; - private double rollZero = 0; - - /** - * Creates a Gyro based on a pigeon - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(Pigeon2 gyro) { - m_pigeon = gyro; - m_isGyroAPigeon = true; - } - - /** - * Creates a Gyro based on a navX - * @param gyro the gyroscope to use for Gyro - */ - public RobotGyro(AHRS gyro){ - m_navX = gyro; - m_isGyroAPigeon = false; - } - - /** - * Resets yaw, pitch, and roll. - */ - public void resetZeroValues() { - if (!m_isGyroAPigeon) return; - - // pitchZero = m_pigeon.getPitch(); - // rollZero = m_pigeon.getRoll(); - } - - /** - * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note - * that the getRate() method for a navX will likely be much more accurate than for a pigeon. - */ - public void updatePigeonDeltas() { - double currentPigeonAngle = getAngle(); - m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle; - m_lastPigeonAngle = currentPigeonAngle; - } - - /** - *

NavX: - *

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

Pigeon: - *

Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot - * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon - * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to - * make sure that the robot is not moving while the tempurature calculations are in progress, this - * is typically done when the robot is first turned on while it's sitting at rest before the - * competition starts. - */ - public void calibrate() { - return; - // if (m_isGyroAPigeon) { - // m_pigeon.calibrate(); - // } else { - // m_navX.calibrate(); - // } - } - - public void reset() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(0); - } else { - m_navX.reset(); - } - - } - - public void reset(double val) { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(val); - } else { - m_navX.reset(); - } - - } - - public void resetFlip() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(180); - } else { - m_navX.reset(); - } - - } - - public void resetNinety() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(90); - } else { - m_navX.reset(); - } - - } - - public void resetTwoSeventy() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(270); - } else { - m_navX.reset(); - } - - } - - public void resetRightSideBlue() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(60); - } else { - m_navX.reset(); - } - - } - - public void resetAmpSide() { - resetZeroValues(); - - if (m_isGyroAPigeon) { - m_pigeon.setYaw(-60); - } else { - m_navX.reset(); - } - - } - - /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. - */ - private double[] getPigeonAngles() { - //m_pigeon.getAngle(); // This appeared to not do anything? - var rotation = m_pigeon.getRotation3d(); - - return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())}; - } - - public Rotation2d getRotation2d() { - return m_pigeon.getRotation2d(); - } - - public double getAngle() { - if (m_isGyroAPigeon) { - return getPigeonAngles()[2]; - } else { - return m_navX.getAngle(); - } - } - - public double getYaw() { - return this.getAngle(); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading() { - return getHeading(getAngle()); - } - - /** - * Gets an absolute heading of the robot - * @return heading from -180 to 180 degrees - */ - public double getHeading(double angle) { - return Math.IEEEremainder(angle, 360); - } - - /** - * Returns the current pitch value (in degrees, from -90 to 90) - * reported by the sensor. Pitch is a measure of rotation around - * the Y Axis. - * @return The current pitch value in degrees (-90 to 90). - */ - public double getPitch() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[1], -90, 90); - } else { - return MathUtil.clamp(m_navX.getPitch(), -90, 90); - } - } - - /** - * Returns the current roll value (in degrees, from -90 to 90) - * reported by the sensor. Roll is a measure of rotation around - * the X Axis. - * @return The current roll value in degrees (-90 to 90). - */ - public double getRoll() { - if (m_isGyroAPigeon) { - return MathUtil.clamp(getPigeonAngles()[2], -90, 90); - } else { - return MathUtil.clamp(m_navX.getRoll(), -90, 90); - } - } - - public double getRate() { - if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; - } else { - return m_navX.getRate(); - } - } - - public Pigeon2 getPigeon(){ - return m_pigeon; - } - - public AHRS getNavX(){ - return m_navX; - } - - public void close() throws Exception { - - } -} diff --git a/src/main/java/frc4388/utility/Subsystem.java b/src/main/java/frc4388/utility/Subsystem.java deleted file mode 100644 index 961900d..0000000 --- a/src/main/java/frc4388/utility/Subsystem.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc4388.utility; - -import java.util.ArrayList; -import java.util.List; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public abstract class Subsystem extends SubsystemBase { - public static List subsystems = new ArrayList<>(); - - public Subsystem(){ - subsystems.add(this); - } - - public void Log(String str) { - System.out.println(getSubsystemName() + " - " + str); - } - - // Get name of subsystem, for use in log. - public abstract String getSubsystemName(); - // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard - public abstract void queryStatus(); - // Proactivly search for any errors in each subsystem - public abstract Status diagnosticStatus(); -} 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 95% rename from src/main/java/frc4388/utility/TimesNegativeOne.java rename to src/main/java/frc4388/utility/compute/TimesNegativeOne.java index bf9818f..2ba0d55 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,7 +7,7 @@ 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.subsystems.swerve.SwerveDriveConstants; // Class that holds weather the drivers sticks should be inverted public class TimesNegativeOne { diff --git a/src/main/java/frc4388/utility/Trim.java b/src/main/java/frc4388/utility/compute/Trim.java similarity index 60% rename from src/main/java/frc4388/utility/Trim.java rename to src/main/java/frc4388/utility/compute/Trim.java index 9b06575..429d526 100644 --- a/src/main/java/frc4388/utility/Trim.java +++ b/src/main/java/frc4388/utility/compute/Trim.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc4388.utility; +package frc4388.utility.compute; import java.io.FileInputStream; import java.io.FileOutputStream; @@ -27,11 +27,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 +104,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/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 4577a2e..ae4202b 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,6 +1,7 @@ + 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/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java new file mode 100644 index 0000000..4065586 --- /dev/null +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -0,0 +1,10 @@ +package frc4388.utility.status; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc4388.robot.RobotContainer; + +// Class to update a series of WPILIB Alerts +public class Alerts { + public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError); +} 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/FaultCANCoder.java b/src/main/java/frc4388/utility/status/FaultCANCoder.java new file mode 100644 index 0000000..a1631fb --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultCANCoder.java @@ -0,0 +1,76 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.CANcoder; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultCANCoder implements Queryable { + private String name; + private CANcoder cancoder; + + public static void addDevice(CANcoder cancoder, String name) { + FaultCANCoder p = new FaultCANCoder(); + + p.name = name; + p.cancoder = cancoder; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage()); + boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + // faults + if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Bad magnet"); + } + if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet"); + } + if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout"); + } + if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultPhotonCamera.java b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java new file mode 100644 index 0000000..5ea8b70 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPhotonCamera.java @@ -0,0 +1,35 @@ +package frc4388.utility.status; + +import org.photonvision.PhotonCamera; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPhotonCamera implements Queryable { + private String name; + private PhotonCamera cam; + + public static void addDevice(PhotonCamera cam, String name) { + FaultPhotonCamera p = new FaultPhotonCamera(); + + p.name = name; + p.cam = cam; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + if(!cam.isConnected()) + s.addReport(ReportLevel.ERROR, "Not Connected!"); + + return s; + } +} + diff --git a/src/main/java/frc4388/utility/status/FaultPidgeon2.java b/src/main/java/frc4388/utility/status/FaultPidgeon2.java new file mode 100644 index 0000000..1e0bb4f --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultPidgeon2.java @@ -0,0 +1,168 @@ +package frc4388.utility.status; + +import com.ctre.phoenix6.controls.EmptyControl; +import com.ctre.phoenix6.hardware.Pigeon2; + +import frc4388.utility.status.Status.ReportLevel; + +public class FaultPidgeon2 implements Queryable { + private String name; + private Pigeon2 pigeon2; + + public static void addDevice(Pigeon2 pigeon2, String name) { + FaultPidgeon2 p = new FaultPidgeon2(); + + p.name = name; + p.pigeon2 = pigeon2; + + FaultReporter.register(p); + } + + @Override + public String getName() { + return name; + } + + @Override + public Status diagnosticStatus() { + Status s = new Status(); + + + + boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage()); + boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0; + + if(debounceBad || emptyControlBad) { + s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + + (debounceBad ? " Failed debounce test" : "") + + (emptyControlBad ? " Failed empty control test" : "") + ); + } + + + s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage()); + + s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch()); + s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw()); + s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll()); + + s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX()); + s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY()); + s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ()); + + s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX()); + s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY()); + s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ()); + + + // faults + if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while enabled"); + } + if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Device booted while in motion"); + } + if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Accelerometer fault detected"); + } + if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro fault detected"); + } + if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Magnetometer fault detected"); + } + if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack data acquisition slower than expected"); + } + if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Hardware fault detected"); + } + if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "Motion stack loop time was slower than expected"); + } + if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Accelerometer values are saturated"); + } + if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Gyro values are saturated"); + } + if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Magnetometer values are saturated"); + } + if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "Device supply voltage near brownout"); + } + if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "Unlicensed feature in use"); + } + + // sticky faults + if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while enabled"); + } + if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Device booted while in motion"); + } + if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Accelerometer fault detected"); + } + if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) { + s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected"); + } + if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Magnetometer fault detected"); + } + if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack data acquisition slower than expected")); + } + if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Hardware fault detected"); + } + if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + String.format( + "[STICKY] Motion stack loop time was slower than expected")); + } + if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Accelerometer values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Gyro values are saturated"); + } + if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Magnetometer values are saturated"); + } + if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, + "[STICKY] Device supply voltage near brownout"); + } + if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) { + s.addReport( + ReportLevel.ERROR, "[STICKY] Unlicensed feature in use"); + } + + return s; + } +} diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java new file mode 100644 index 0000000..afd4dd4 --- /dev/null +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -0,0 +1,133 @@ +package frc4388.utility.status; + +import java.util.ArrayList; +import java.util.List; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; + +import frc4388.robot.constants.Constants; +import frc4388.utility.status.Status.Report; +import frc4388.utility.status.Status.ReportLevel; + +public class FaultReporter { + + private static final String REPORTS_HEADER = + "###############\n" + // + "#.............#\n" + // + "#...Reports...#\n" + // + "#.............#\n" + // + "###############\n"; + + + private static final String CAN_HEADER = + "###############\n" + // + "#.............#\n" + // + "#....CAN(t)...#\n" + // + "#.............#\n" + // + "###############\n"; + + private static final String ERROR_HEADER = + "###############\n" + // + "#.............#\n" + // + "#....ERRORS...#\n" + // + "#.............#\n" + // + "###############\n"; + + private static List queryables = new ArrayList<>(); + + // public static void startThread() { + // new Thread() { + // public void run() { + // try{ + // while(!this.isInterrupted() && this.isAlive()){ + // Thread.sleep(500); + // for(int i=0;i errors = new ArrayList<>(); + + // Subsystems header + System.out.println(REPORTS_HEADER); + + for(int i=0;i< queryables.size();i++){ + + Queryable q = queryables.get(i); + System.out.println("** Subsystem diagnostic report for " + q.getName() + ":"); + Status status = q.diagnosticStatus(); + + for(int a=0;a 0) { + // Errors header + System.out.println(ERROR_HEADER); + for(int i=0;i