From b994219ab7b219a11da198afd0b327aaa9c7f753 Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Thu, 4 Dec 2025 20:35:43 -0800 Subject: [PATCH 01/11] applied fixes for issue #215 --- src/main/java/frc/robot/Constants.java | 237 ++++++++++++++++++ src/main/java/frc/robot/Controls.java | 59 +++-- src/main/java/frc/robot/Hardware.java | 52 ---- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/Subsystems.java | 10 +- .../java/frc/robot/sensors/ArmSensor.java | 4 +- .../java/frc/robot/sensors/BranchSensors.java | 6 +- .../java/frc/robot/sensors/ElevatorLight.java | 4 +- .../java/frc/robot/sensors/IntakeSensor.java | 4 +- .../java/frc/robot/subsystems/ArmPivot.java | 66 ++--- .../java/frc/robot/subsystems/ClimbPivot.java | 78 +++--- .../robot/subsystems/ElevatorSubsystem.java | 74 ++---- .../java/frc/robot/subsystems/GroundArm.java | 43 ++-- .../frc/robot/subsystems/GroundSpinny.java | 39 +-- .../java/frc/robot/subsystems/SpinnyClaw.java | 40 ++- .../frc/robot/subsystems/SuperStructure.java | 178 +++++++------ .../frc/robot/subsystems/VisionSubsystem.java | 61 +---- 17 files changed, 512 insertions(+), 446 deletions(-) create mode 100644 src/main/java/frc/robot/Constants.java delete mode 100644 src/main/java/frc/robot/Hardware.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 00000000..c4949a47 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,237 @@ +package frc.robot; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; + +public class Constants { + public class armPivotConstants { + // Presets + public static final double ARMPIVOT_KP = 38.5; // previously 50 + public static final double ARMPIVOT_KI = 0; + public static final double ARMPIVOT_KD = 0; + public static final double ARMPIVOT_KS = 0.1; + public static final double ARMPIVOT_KV = 0.69; + public static final double ARMPIVOT_KG = 0.18; + public static final double ARMPIVOT_KA = 0.0; + // Preset positions for Arm with Coral + public static final double ARMPIVOT_CORAL_PRESET_L1 = 0; + public static final double ARMPIVOT_CORAL_PRESET_L2 = 0.13; + public static final double ARMPIVOT_CORAL_PRESET_L3 = 0.13; + public static final double ARMPIVOT_CORAL_PRESET_L4 = 0.0; + public static final double ARMPIVOT_CORAL_PRESET_PRE_L4 = 1.0 / 16.0; + public static final double ARMPIVOT_CORAL_PRESET_STOWED = 0.125; + public static final double ARMPIVOT_CORAL_PRESET_OUT = 0; + public static final double ARMPIVOT_CORAL_PRESET_UP = 0.25; // Pointing directly upwards + public static final double ARMPIVOT_CORAL_PRESET_DOWN = -0.25; + // Preset positions for Arm with Algae + public static final double ARMPIVOT_CORAL_POST_SCORE = -0.15; + public static final double ARMPIVOT_CORAL_QUICK_INTAKE = -0.07; + public static final double ARMPIVOT_ALGAE_REMOVE_PREPOS = 0; + public static final double ARMPIVOT_ALGAE_REMOVE = 0; + public static final double ARMPIVOT_ALGAE_FLING = -0.08; + public static final double ARMPIVOT_ALGAE_STOWED = -0.05; + public static final double ARMPIVOT_ALGAE_PROCESSOR_SCORE = -0.05; + public static final double ARMPIVOT_ALGAE_GROUND_INTAKE = -0.085; + public static final double ARMPIVOT_ALGAE_NET_SCORE = 0.175; // untested - old value was 0.18 + + // Other Presets + public static final double ARMPIVOT_CORAL_PRESET_GROUND_INTAKE = 0; + public static final double ARMPIVOT_HARDSTOP_HIGH = 0.32; + public static final double ARMPIVOT_HARDSTOP_LOW = -0.26; + public static final double ARMPIVOT_POS_TOLERANCE = Units.degreesToRotations(5); + public static final double ARMPIVOT_PLACEHOLDER_CORAL_WEIGHT_KG = 0.8; + // Constant for gear ratio (the power that one motor gives to gear) + public static final double ARM_RATIO = (12.0 / 60.0) * (20.0 / 60.0) * (18.0 / 48.0); + // arm pivot [30-34] + public static final int ARM_PIVOT_MOTOR_ID = 30; + public static final int ARM_PIVOT_CANDI_ID = 31; + // arm Sensors [35-39] + public static final int MAIN_ARM_SENSOR = 35; + } + + public class climbConstants { + // various position and speed presets + public static final double STOWED_MAX_PRESET = -0.447; + public static final double STOWED_MIN_PRESET = -0.450; + public static final double CLIMB_OUT_MAX_PRESET = -0.150; + public static final double CLIMB_OUT_MIN_PRESET = -0.177; + public static final double CLIMBED_MAX_PRESET = -0.325; + public static final double CLIMBED_MIN_PRESET = -0.34; + public static final double FORWARD_SOFT_STOP = -0.07; + public static final double REVERSE_SOFT_STOP = -78; + public static final double CLIMB_OUT_SPEED = 1.0; + public static final double CLIMB_HOLD_STOWED = -0.001; + public static final double CLIMB_HOLD_CLIMBOUT = -0.0; + public static final double CLIMB_HOLD_CLIMBED = -0.0705; + public static final double CLIMB_IN_SPEED = -0.75; + public static final double CLIMB_STATOR_CURRENT_LIMIT = 150; + public static final double CLIMB_SUPPLY_CURRENT_LIMIT = 75; + + public static final double climbInKp = 50; + public static final double climbOutKp = 50; + + // positions for relation between motor encoder and WCP encoder + // relative to eachother, likely not accurately zero'ed when obtained. + public static final double MIN_ROTOR_POSITION = -50.45; + public static final double MAX_ROTOR_POSITION = 14.456; + public static final double MIN_ENCODER_POSITION = 0.611; + public static final double MAX_ENCODER_POSITION = 0.915; + + // climb DIO + public static final int CLIMB_SENSOR = 8; + + // climb [50-59] + public static final int CLIMB_PIVOT_MOTOR_LEFT_ID = 50; + public static final int CLIMB_PIVOT_MOTOR_RIGHT_ID = 51; + public static final int CLIMB_PIVOT_CANCODER_ID = 52; + } + + public class elevatorConstants { + // Maximum is 38.34 + public static final double CORAL_LEVEL_FOUR_PRE_POS = 37.5; + public static final double CORAL_LEVEL_FOUR_POS = 36; + public static final double CORAL_LEVEL_THREE_PRE_POS = 18.65; + public static final double CORAL_LEVEL_THREE_POS = 14; + public static final double CORAL_LEVEL_TWO_PRE_POS = 6.94; + public static final double CORAL_LEVEL_TWO_POS = 4.4; + public static final double CORAL_LEVEL_ONE_POS = 4.2; + public static final double ALGAE_LEVEL_TWO_THREE = 11; + public static final double ALGAE_LEVEL_TWO_THREE_FLING = 16; + public static final double ALGAE_LEVEL_THREE_FOUR = 21; + public static final double ALGAE_LEVEL_THREE_FOUR_FLING = 25; + public static final double ALGAE_STOWED = 2; + public static final double ALGAE_PROCESSOR_SCORE = 2; + public static final double ALGAE_NET_SCORE = 38; // untested + public static final double ALGAE_GROUND_INTAKE = 0.05; + public static final double CORAL_STOWED = CORAL_LEVEL_TWO_PRE_POS; + public static final double CORAL_GROUND_INTAKE_POS = 7.2; + public static final double CORAL_INTAKE_POS = 1.55; + public static final double CORAL_PRE_INTAKE = 4.7; + public static final double CORAL_QUICK_INTAKE = 1.6; + public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; + public static final double MIN_FULL_GROUND_INTAKE = 8.0; + public static final double MANUAL = 0.1; + public static final double POS_TOLERANCE = 0.1; + public static final double ELEVATOR_KP = 7.804; + public static final double ELEVATOR_KI = 0; + public static final double ELEVATOR_KD = 0.079221; + public static final double ELEVATOR_KS = 0.33878; + public static final double ELEVATOR_KV = 0.12975; + public static final double ELEVATOR_KA = 0.0070325; + public static final double REVERSE_SOFT_LIMIT = -0.05; + public static final double FORWARD_SOFT_LIMIT = 38; + public static final double UP_VOLTAGE = 5; + public static final double DOWN_VOLTAGE = -3; + public static final double HOLD_VOLTAGE = 0.6; + public static final double CURRENT_STATOR_LIMIT = 160; + public static final double CURRENT_SUPPLY_LIMIT = 80; + + // elevator zero button DIO + public static final int ELEVATOR_ZERO_BUTTON = 0; + // elevator [20-24] + public static final int ELEVATOR_MOTOR_ONE = 20; + public static final int ELEVATOR_MOTOR_TWO = 21; + } + + public class groundArmConstants { + public static final double ARMPIVOT_KP = 40; + public static final double ARMPIVOT_KI = 0; + public static final double ARMPIVOT_KD = 0; + public static final double ARMPIVOT_KS = 0.9; + public static final double ARMPIVOT_KV = 4; + public static final double ARMPIVOT_KG = 0.048; + public static final double ARMPIVOT_KA = 0; + public static final double STOWED_POSITION = 0.46; + public static final double UP_POSITION = + 0.27; // untested - should be somewhere in between stowed and ground + public static final double GROUND_POSITION = -0.050; + public static final double QUICK_INTAKE_POSITION = 0.31; + public static final double POS_TOLERANCE = Units.degreesToRotations(5); + public static final double GROUND_HOLD_VOLTAGE = -0.40; + // ratio from motor rotations to output rotations + public static final double ARM_RATIO = 60; + // ground intake [45-49] + public static final int GROUND_INTAKE_ARM_MOTOR = 45; + public static final int GROUND_INTAKE_ARM_ENCODER = 47; + public static final int GROUND_INTAKE_SENSOR = 48; + } + + public class groundSpinnyConstants { + public static final double GROUND_INTAKE_SPEED = -8; + public static final double GROUND_INTAKE_JITTER_SPEED = 1; + public static final double FUNNEL_INTAKE_SPEED = -2; + public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; + public static final double STATOR_CURRENT_STALL_THRESHOLD = 50; + public static final int GROUND_INTAKE_SPINNY_MOTOR = 46; + } + + public class spinnyClawConstants { + public static final double CORAL_INTAKE_SPEED = -6; + public static final double CORAL_EXTAKE_SPEED = 5; + public static final double CORAL_REJECT_SPEED = 1; + public static final double CORAL_L1_EXTAKE_SPEED = 1.7; + public static final double ALGAE_INTAKE_SPEED = -4; // started at -3.5 + public static final double ALGAE_GRIP_INTAKE_SPEED = -3; // started at -2.5 + public static final double ALGAE_EXTAKE_SPEED = 14; + public static final double ALGAE_PROCESSOR_EXTAKE_SPEED = 8; + public static final double ALGAE_FLING_SPEED = 10; + // spinny claw [40-44] + public static final int SPINNY_CLAW_MOTOR_ID = 40; + } + + public class visionConstants_Misc { + // differences from center robot camera poses + private static final double CAMERA_X_POS_METERS_LEFT = 0.26; + private static final double CAMERA_X_POS_METERS_RIGHT = 0.27; + private static final double CAMERA_Y_POS_METERS_LEFT = 0.25; + private static final double CAMERA_Y_POS_METERS_RIGHT = -0.25; + private static final double CAMERA_Z_POS_METERS_LEFT = 0.20; + private static final double CAMERA_Z_POS_METERS_RIGHT = 0.21; + private static final double CAMERA_ROLL_LEFT = Units.degreesToRadians(3); + private static final double CAMERA_ROLL_RIGHT = Units.degreesToRadians(0.92); + private static final double CAMERA_PITCH_LEFT = Units.degreesToRadians(-6.3); + private static final double CAMERA_PITCH_RIGHT = Units.degreesToRadians(-8.3); + private static final double CAMERA_YAW_LEFT = Units.degreesToRadians(-44.64); + private static final double CAMERA_YAW_RIGHT = Units.degreesToRadians(46.42); + // left camera diffrences from center robot + public static final Transform3d ROBOT_TO_CAM_LEFT = + new Transform3d( + // Translation3d.kZero, + CAMERA_X_POS_METERS_LEFT, + CAMERA_Y_POS_METERS_LEFT, + CAMERA_Z_POS_METERS_LEFT, + // Rotation3d.kZero); + new Rotation3d(CAMERA_ROLL_LEFT, CAMERA_PITCH_LEFT, CAMERA_YAW_LEFT)); + // right camera diffrences from center robot + public static final Transform3d ROBOT_TO_CAM_RIGHT = + new Transform3d( + // Translation3d.kZero, + CAMERA_X_POS_METERS_RIGHT, + CAMERA_Y_POS_METERS_RIGHT, + CAMERA_Z_POS_METERS_RIGHT, + // Rotation3d.kZero); + new Rotation3d(CAMERA_ROLL_RIGHT, CAMERA_PITCH_RIGHT, CAMERA_YAW_RIGHT)); + + // Deviations + public static final Vector STANDARD_DEVS = + VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(20)); + public static final Vector DISTANCE_SC_STANDARD_DEVS = + VecBuilder.fill(1, 1, Units.degreesToRadians(50)); + public static final String PHOTON_IP = "10.24.12.11"; + public static final String LEFT_CAM = "Arducam_OV9782L"; + public static final String RIGHT_CAM = "Arducam_OV9281R"; + // branch sensors [25-29] (mounted on elevator) + // fixed it for u connor ;) - mech + public static final int BRANCH_SENSOR_LEFT = 25; + public static final int BRANCH_SENSOR_RIGHT = 26; + public static final int PDH_ID = 1; + // Swerve: 1-12 + + // LEDs [15-19] + public static final int ELEVATOR_LED = 15; + } +} diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0adda27d..b3747555 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -24,11 +24,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.*; +import frc.robot.Constants.armPivotConstants; +import frc.robot.Constants.elevatorConstants; +import frc.robot.Constants.groundArmConstants; import frc.robot.generated.BonkTunerConstants; import frc.robot.generated.CompTunerConstants; -import frc.robot.subsystems.ArmPivot; -import frc.robot.subsystems.ElevatorSubsystem; -import frc.robot.subsystems.GroundArm; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoAlgaeHeights; import frc.robot.subsystems.auto.AutoAlign; @@ -513,7 +514,7 @@ private void configureElevatorBindings() { .whileTrue( s.elevatorSubsystem .startMovingVoltage( - () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -operatorController.getLeftY())) + () -> Volts.of(elevatorConstants.UP_VOLTAGE * -operatorController.getLeftY())) .withName("Elevator Manual Control")); s.elevatorSubsystem.setRumble( (rumble) -> { @@ -524,55 +525,55 @@ private void configureElevatorBindings() { .and(elevatorTestController.y()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_FOUR_POS) .withName("Elevator L4")); connected(elevatorTestController) .and(elevatorTestController.x()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_THREE_POS) .withName("Elevator L3")); connected(elevatorTestController) .and(elevatorTestController.b()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_TWO_POS) .withName("Elevator L2")); connected(elevatorTestController) .and(elevatorTestController.a()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS) .withName("Elevator L1")); connected(elevatorTestController) .and(elevatorTestController.rightBumper()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.CORAL_INTAKE_POS) + .setLevel(elevatorConstants.CORAL_INTAKE_POS) .withName("Elevator IntakePos")); connected(elevatorTestController) .and(elevatorTestController.povUp()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR) + .setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR) .withName("Elevator Algae L3-L4")); connected(elevatorTestController) .and(elevatorTestController.povLeft()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE) + .setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE) .withName("Elevator Algae L2-L3")); connected(elevatorTestController) .and(elevatorTestController.povRight()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.ALGAE_STOWED) + .setLevel(elevatorConstants.ALGAE_STOWED) .withName("Elevator Algae Stowed")); connected(elevatorTestController) .and(elevatorTestController.povDown()) .onTrue( s.elevatorSubsystem - .setLevel(ElevatorSubsystem.ALGAE_PROCESSOR_SCORE) + .setLevel(elevatorConstants.ALGAE_PROCESSOR_SCORE) .withName("Elevator Processor")); connected(elevatorTestController) .and(elevatorTestController.leftBumper()) @@ -593,7 +594,7 @@ private void configureElevatorBindings() { .schedule(); } - // operatorController.rightBumper().whileTrue(s.elevatorSubsystem.holdCoastMode()); + // operatorController.rightBumper().whileTrue(s.elevatorConstants.holdCoastMode()); var elevatorCoastButton = Shuffleboard.getTab("Controls") .add("Elevator Coast Mode", false) @@ -605,7 +606,7 @@ private void configureElevatorBindings() { // new Trigger(() -> elevatorZeroButton.get()) // .debounce(1, DebounceType.kRising) // .and(RobotModeTriggers.disabled()) - // .onTrue(s.elevatorSubsystem.resetPosZero()); + // .onTrue(s.elevatorConstants.resetPosZero()); } private void configureArmPivotBindings() { @@ -629,38 +630,44 @@ private void configureArmPivotBindings() { connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povRight()) .onTrue( - s.armPivotSubsystem.moveToPosition(ArmPivot.CORAL_PRESET_L4).withName("Arm L4 Preset")); + s.armPivotSubsystem + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L4) + .withName("Arm L4 Preset")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povLeft()) .onTrue( - s.armPivotSubsystem.moveToPosition(ArmPivot.CORAL_PRESET_L3).withName("Arm L3 Preset")); + s.armPivotSubsystem + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) + .withName("Arm L3 Preset")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povUp()) .onTrue( - s.armPivotSubsystem.moveToPosition(ArmPivot.CORAL_PRESET_UP).withName("Arm Preset Up")); + s.armPivotSubsystem + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .withName("Arm Preset Up")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povDown()) .onTrue( s.armPivotSubsystem - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withName("Arm Preset Down")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.y()) .onTrue( s.armPivotSubsystem - .moveToPosition(ArmPivot.ALGAE_REMOVE) + .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE) .withName("Algae Preset Remove")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.b()) .onTrue( s.armPivotSubsystem - .moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE) + .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE) .withName("Algae Preset Score")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.a()) .onTrue( s.armPivotSubsystem - .moveToPosition(ArmPivot.ALGAE_STOWED) + .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_STOWED) .withName("Algae Preset Stowed")); } @@ -855,14 +862,14 @@ private void configureGroundArmBindings() { } s.groundArm.setDefaultCommand( s.groundArm - .moveToPosition(GroundArm.STOWED_POSITION) + .moveToPosition(groundArmConstants.STOWED_POSITION) .andThen(Commands.idle()) .withName("Ground stowed position wait")); operatorController .rightBumper() .whileTrue( s.groundArm - .moveToPosition(GroundArm.GROUND_POSITION) + .moveToPosition(groundArmConstants.GROUND_POSITION) .andThen(Commands.idle()) .withName("ground up position")); } @@ -1116,7 +1123,7 @@ private void configureSoloControllerBindings() { .povUp() .whileTrue( s.groundArm - .moveToPosition(GroundArm.GROUND_POSITION) + .moveToPosition(groundArmConstants.GROUND_POSITION) .andThen(Commands.idle()) .withName("ground up position")); // Arm manual @@ -1132,7 +1139,7 @@ private void configureSoloControllerBindings() { .whileTrue( s.elevatorSubsystem .startMovingVoltage( - () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) + () -> Volts.of(elevatorConstants.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); } } diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java deleted file mode 100644 index 08553e5f..00000000 --- a/src/main/java/frc/robot/Hardware.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot; - -public class Hardware { - // Add motor IDs here - - public static final int PDH_ID = 1; - // Swerve: 1-12 - - // LEDs [15-19] - public static final int ELEVATOR_LED = 15; - - // elevator [20-24] - public static final int ELEVATOR_MOTOR_ONE = 20; - public static final int ELEVATOR_MOTOR_TWO = 21; - - // branch sensors [25-29] (mounted on elevator) - // fixed it for u connor ;) - mech - public static final int BRANCH_SENSOR_LEFT = 25; - public static final int BRANCH_SENSOR_RIGHT = 26; - - // arm pivot [30-34] - public static final int ARM_PIVOT_MOTOR_ID = 30; - public static final int ARM_PIVOT_CANDI_ID = 31; - - // arm Sensors [35-39] - public static final int MAIN_ARM_SENSOR = 35; - - // spinny claw [40-44] - public static final int SPINNY_CLAW_MOTOR_ID = 40; - - // ground intake [45-49] - public static final int GROUND_INTAKE_ARM_MOTOR = 45; - public static final int GROUND_INTAKE_SPINNY_MOTOR = 46; - public static final int GROUND_INTAKE_ARM_ENCODER = 47; - public static final int GROUND_INTAKE_SENSOR = 48; - - // climb [50-59] - public static final int CLIMB_PIVOT_MOTOR_LEFT_ID = 50; - public static final int CLIMB_PIVOT_MOTOR_RIGHT_ID = 51; - public static final int CLIMB_PIVOT_CANCODER_ID = 52; - - // elevator zero button DIO - public static final int ELEVATOR_ZERO_BUTTON = 0; - - // climb DIO - public static final int CLIMB_SENSOR = 8; - - // vision - public static final String PHOTON_IP = "10.24.12.11"; - public static final String LEFT_CAM = "Arducam_OV9782L"; - public static final String RIGHT_CAM = "Arducam_OV9281R"; -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..5e9452b4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants.visionConstants_Misc; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoBuilderConfig; @@ -49,7 +50,7 @@ protected Robot() { instance = this; robotType = RobotType.getCurrent(); CanBridge.runTCP(); - PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); + PDH = new PowerDistribution(visionConstants_Misc.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 76a24f44..75d4e072 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -1,6 +1,14 @@ package frc.robot; -import static frc.robot.Subsystems.SubsystemConstants.*; +import static frc.robot.Subsystems.SubsystemConstants.ARMPIVOT_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.CLIMBPIVOT_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.ELEVATOR_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.ELEVATOR_LED_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.GROUND_ARM_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.GROUND_SPINNY_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.SPINNYCLAW_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.VISION_ENABLED; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.generated.BonkTunerConstants; diff --git a/src/main/java/frc/robot/sensors/ArmSensor.java b/src/main/java/frc/robot/sensors/ArmSensor.java index 49becd21..345738a7 100644 --- a/src/main/java/frc/robot/sensors/ArmSensor.java +++ b/src/main/java/frc/robot/sensors/ArmSensor.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.armPivotConstants; public class ArmSensor { @@ -22,7 +22,7 @@ public class ArmSensor { private static final double CLAW_UPPER_LIMIT = 0.13; public ArmSensor() { - mainSensor = new LaserCan(Hardware.MAIN_ARM_SENSOR); + mainSensor = new LaserCan(armPivotConstants.MAIN_ARM_SENSOR); ConfigureSensor(mainSensor); ShuffleboardTab tab = Shuffleboard.getTab("ArmSensor"); tab.addBoolean("inTrough", inTrough()); diff --git a/src/main/java/frc/robot/sensors/BranchSensors.java b/src/main/java/frc/robot/sensors/BranchSensors.java index a9735843..47d34755 100644 --- a/src/main/java/frc/robot/sensors/BranchSensors.java +++ b/src/main/java/frc/robot/sensors/BranchSensors.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.visionConstants_Misc; public class BranchSensors { @@ -23,8 +23,8 @@ public class BranchSensors { private static final double LEFT_SENSOR_MAX = 0.35; public BranchSensors() { - leftSensor = new LaserCan(Hardware.BRANCH_SENSOR_LEFT); - rightSensor = new LaserCan(Hardware.BRANCH_SENSOR_RIGHT); + leftSensor = new LaserCan(visionConstants_Misc.BRANCH_SENSOR_LEFT); + rightSensor = new LaserCan(visionConstants_Misc.BRANCH_SENSOR_RIGHT); ConfigureSensor(leftSensor); ConfigureSensor(rightSensor); ShuffleboardTab tab = Shuffleboard.getTab("BranchSensor"); diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index f6139f74..3809a678 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Hardware; +import frc.robot.Constants.visionConstants_Misc; import frc.robot.util.SoloScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -39,7 +39,7 @@ public class ElevatorLight extends SubsystemBase { public ElevatorLight() { - candle = new CANdle(Hardware.ELEVATOR_LED); + candle = new CANdle(visionConstants_Misc.ELEVATOR_LED); configureCandle(); candle.clearAnimation(0); candle.setLEDs(128, 128, 128); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 5d77eddc..9759ef9a 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.groundArmConstants; public class IntakeSensor { @@ -19,7 +19,7 @@ public class IntakeSensor { private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { - intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); + intakeSensor = new LaserCan(groundArmConstants.GROUND_INTAKE_SENSOR); ConfigureSensor(intakeSensor); ShuffleboardTab tab = Shuffleboard.getTab("IntakeSensor"); tab.addBoolean("In Intake", inIntake()); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index bdb98cae..e8cfa58e 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -27,47 +26,10 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.Hardware; +import frc.robot.Constants.armPivotConstants; import java.util.function.Supplier; public class ArmPivot extends SubsystemBase { - // Presets - private final double ARMPIVOT_KP = 38.5; // previously 50 - private final double ARMPIVOT_KI = 0; - private final double ARMPIVOT_KD = 0; - private final double ARMPIVOT_KS = 0.1; - private final double ARMPIVOT_KV = 0.69; - private final double ARMPIVOT_KG = 0.18; - private final double ARMPIVOT_KA = 0.0; - // Preset positions for Arm with Coral - public static final double CORAL_PRESET_L1 = 0; - public static final double CORAL_PRESET_L2 = 0.13; - public static final double CORAL_PRESET_L3 = 0.13; - public static final double CORAL_PRESET_L4 = 0.0; - public static final double CORAL_PRESET_PRE_L4 = 1.0 / 16.0; - public static final double CORAL_PRESET_STOWED = 0.125; - public static final double CORAL_PRESET_OUT = 0; - public static final double CORAL_PRESET_UP = 0.25; // Pointing directly upwards - public static final double CORAL_PRESET_DOWN = -0.25; - // Preset positions for Arm with Algae - public static final double CORAL_POST_SCORE = -0.15; - public static final double CORAL_QUICK_INTAKE = -0.07; - public static final double ALGAE_REMOVE_PREPOS = 0; - public static final double ALGAE_REMOVE = 0; - public static final double ALGAE_FLING = -0.08; - public static final double ALGAE_STOWED = -0.05; - public static final double ALGAE_PROCESSOR_SCORE = -0.05; - public static final double ALGAE_GROUND_INTAKE = -0.085; - public static final double ALGAE_NET_SCORE = 0.175; // untested - old value was 0.18 - - // Other Presets - public static final double CORAL_PRESET_GROUND_INTAKE = 0; - public static final double HARDSTOP_HIGH = 0.32; - public static final double HARDSTOP_LOW = -0.26; - public static final double POS_TOLERANCE = Units.degreesToRotations(5); - public static final double PLACEHOLDER_CORAL_WEIGHT_KG = 0.8; - // Constant for gear ratio (the power that one motor gives to gear) - private static final double ARM_RATIO = (12.0 / 60.0) * (20.0 / 60.0) * (18.0 / 48.0); // create a Motion Magic request, voltage output private final MotionMagicVoltage m_request = new MotionMagicVoltage(0); @@ -86,7 +48,7 @@ public class ArmPivot extends SubsystemBase { // Arm Pivot Contructor public ArmPivot() { - motor = new TalonFX(Hardware.ARM_PIVOT_MOTOR_ID); + motor = new TalonFX(armPivotConstants.ARM_PIVOT_MOTOR_ID); routine = new SysIdRoutine( new SysIdRoutine.Config(Volts.of(1).div(Seconds.of(1)), Volts.of(1), Seconds.of(2)), @@ -127,7 +89,8 @@ private Command setTargetPosition(double pos) { // Boolean returning whether or not the arm is at the desired position public boolean atPosition(double position) { - return MathUtil.isNear(position, getCurrentPosition(), POS_TOLERANCE); + return MathUtil.isNear( + position, getCurrentPosition(), armPivotConstants.ARMPIVOT_POS_TOLERANCE); } // Returns the current target position (position the arm is to move to) @@ -155,7 +118,8 @@ public Command moveToPosition(double position) { } public Trigger atAngle(double position) { - return new Trigger(() -> Math.abs(getCurrentPosition() - position) < POS_TOLERANCE); + return new Trigger( + () -> Math.abs(getCurrentPosition() - position) < armPivotConstants.ARMPIVOT_POS_TOLERANCE); } // (+) is to move arm up, and (-) is down. sets a voltage to pass to motor to move @@ -185,9 +149,9 @@ public void factoryDefaults() { var talonFXConfiguration = new TalonFXConfiguration(); // specifies what the sensor is, what port its on, and what the gearing ratio for the sensor is // relative to the motor - talonFXConfiguration.Feedback.FeedbackRemoteSensorID = Hardware.ARM_PIVOT_CANDI_ID; + talonFXConfiguration.Feedback.FeedbackRemoteSensorID = armPivotConstants.ARM_PIVOT_CANDI_ID; talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANdiPWM1; - talonFXConfiguration.Feedback.RotorToSensorRatio = 1 / ARM_RATIO; + talonFXConfiguration.Feedback.RotorToSensorRatio = 1 / armPivotConstants.ARM_RATIO; // Inverting motor output direction talonFXConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; @@ -202,13 +166,13 @@ public void factoryDefaults() { // PID // set slot 0 gains - talonFXConfiguration.Slot0.kS = ARMPIVOT_KS; - talonFXConfiguration.Slot0.kV = ARMPIVOT_KV; - talonFXConfiguration.Slot0.kA = ARMPIVOT_KA; - talonFXConfiguration.Slot0.kP = ARMPIVOT_KP; - talonFXConfiguration.Slot0.kI = ARMPIVOT_KI; - talonFXConfiguration.Slot0.kD = ARMPIVOT_KD; - talonFXConfiguration.Slot0.kG = ARMPIVOT_KG; + talonFXConfiguration.Slot0.kS = armPivotConstants.ARMPIVOT_KS; + talonFXConfiguration.Slot0.kV = armPivotConstants.ARMPIVOT_KV; + talonFXConfiguration.Slot0.kA = armPivotConstants.ARMPIVOT_KA; + talonFXConfiguration.Slot0.kP = armPivotConstants.ARMPIVOT_KP; + talonFXConfiguration.Slot0.kI = armPivotConstants.ARMPIVOT_KI; + talonFXConfiguration.Slot0.kD = armPivotConstants.ARMPIVOT_KD; + talonFXConfiguration.Slot0.kG = armPivotConstants.ARMPIVOT_KG; talonFXConfiguration.Slot0.GravityType = GravityTypeValue.Arm_Cosine; // set Motion Magic settings in rps not mechanism units diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 260e962c..1b3c2884 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.climbConstants; import java.util.function.DoubleSupplier; public class ClimbPivot extends SubsystemBase { @@ -41,40 +41,15 @@ public enum TargetPositions { // variable for the shuffleboard tab for consistency private final ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("Climb"); - // various position and speed presets - private final double STOWED_MAX_PRESET = -0.447; - private final double STOWED_MIN_PRESET = -0.450; - private final double CLIMB_OUT_MAX_PRESET = -0.150; - private final double CLIMB_OUT_MIN_PRESET = -0.177; - private final double CLIMBED_MAX_PRESET = -0.325; - private final double CLIMBED_MIN_PRESET = -0.34; - private final double FORWARD_SOFT_STOP = -0.07; - private final double REVERSE_SOFT_STOP = -78; - private final double CLIMB_OUT_SPEED = 1.0; - private final double CLIMB_HOLD_STOWED = -0.001; - private final double CLIMB_HOLD_CLIMBOUT = -0.0; - private final double CLIMB_HOLD_CLIMBED = -0.0705; - private final double CLIMB_IN_SPEED = -0.75; - - private final double climbInKp = 50; - private final double climbOutKp = 50; - - // positions for relation between motor encoder and WCP encoder - // relative to eachother, likely not accurately zero'ed when obtained. - private static final double MIN_ROTOR_POSITION = -50.45; - private static final double MAX_ROTOR_POSITION = 14.456; - private static final double MIN_ENCODER_POSITION = 0.611; - private static final double MAX_ENCODER_POSITION = 0.915; - // two status variables public boolean isClimbOut = false; private boolean isStowed = true; // setting the starting target position private TargetPositions selectedPos = TargetPositions.STOWED; - private double maxTargetPos = STOWED_MAX_PRESET; - private double minTargetPos = STOWED_MIN_PRESET; - private double holdSpeed = CLIMB_HOLD_STOWED; + private double maxTargetPos = climbConstants.STOWED_MAX_PRESET; + private double minTargetPos = climbConstants.STOWED_MIN_PRESET; + private double holdSpeed = climbConstants.CLIMB_HOLD_STOWED; // if moveComplete is true it wont move regardless of if its in range. This is to ensure that // when disabled, when re-enabled it doesnt start moving. @@ -99,17 +74,17 @@ public enum TargetPositions { // here we go public ClimbPivot() { - motorLeft = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_LEFT_ID); + motorLeft = new TalonFX(climbConstants.CLIMB_PIVOT_MOTOR_LEFT_ID); // checking to instantiate the 2nd motor if two are installed. If only one is installed, the 2nd // one is set to null. if (DUAL_MOTORS) { - motorRight = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_RIGHT_ID); + motorRight = new TalonFX(climbConstants.CLIMB_PIVOT_MOTOR_RIGHT_ID); } else { motorRight = null; } // the failed climb sensor - sensor = new DigitalInput(Hardware.CLIMB_SENSOR); + sensor = new DigitalInput(climbConstants.CLIMB_SENSOR); // motor configuration and shuffleboard logging methods configure(); @@ -136,16 +111,17 @@ private void configure() { TalonFXConfiguration configuration = new TalonFXConfiguration(); // remote sensor values for the WCP encoder - configuration.Feedback.FeedbackRemoteSensorID = Hardware.CLIMB_PIVOT_CANCODER_ID; + configuration.Feedback.FeedbackRemoteSensorID = climbConstants.CLIMB_PIVOT_CANCODER_ID; configuration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; // previously mentioned ratio calculation configuration.Feedback.RotorToSensorRatio = - (MAX_ROTOR_POSITION - MIN_ROTOR_POSITION) / (MAX_ENCODER_POSITION - MIN_ENCODER_POSITION); + (climbConstants.MAX_ROTOR_POSITION - climbConstants.MIN_ROTOR_POSITION) + / (climbConstants.MAX_ENCODER_POSITION - climbConstants.MIN_ENCODER_POSITION); // Set and enable current limit - configuration.CurrentLimits.StatorCurrentLimit = 150; + configuration.CurrentLimits.StatorCurrentLimit = climbConstants.CLIMB_STATOR_CURRENT_LIMIT; configuration.CurrentLimits.StatorCurrentLimitEnable = true; - configuration.CurrentLimits.SupplyCurrentLimit = 75; + configuration.CurrentLimits.SupplyCurrentLimit = climbConstants.CLIMB_SUPPLY_CURRENT_LIMIT; configuration.CurrentLimits.SupplyCurrentLimitEnable = true; // Enable brake mode configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -161,8 +137,8 @@ private void configure() { // having softlimits enabled on both motors isnt great configuration.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; configuration.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - configuration.SoftwareLimitSwitch.ForwardSoftLimitThreshold = FORWARD_SOFT_STOP; - configuration.SoftwareLimitSwitch.ReverseSoftLimitThreshold = REVERSE_SOFT_STOP; + configuration.SoftwareLimitSwitch.ForwardSoftLimitThreshold = climbConstants.FORWARD_SOFT_STOP; + configuration.SoftwareLimitSwitch.ReverseSoftLimitThreshold = climbConstants.REVERSE_SOFT_STOP; talonFXConfigurator.apply(configuration); // OpposeMasterDirection can be changed based on climb design, not yet sure if 2nd motor will be // on opposite side @@ -180,26 +156,26 @@ private void setTargetPos(TargetPositions newTargetPos) { // if stowed, it sets the pos its going to as stowed, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.STOWED; - maxTargetPos = STOWED_MAX_PRESET; - minTargetPos = STOWED_MIN_PRESET; + maxTargetPos = climbConstants.STOWED_MAX_PRESET; + minTargetPos = climbConstants.STOWED_MIN_PRESET; moveComplete = false; } case CLIMB_OUT -> { // if climb_out, it sets the pos its going to as climb_out, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMB_OUT; - maxTargetPos = CLIMB_OUT_MAX_PRESET; - minTargetPos = CLIMB_OUT_MIN_PRESET; - holdSpeed = CLIMB_HOLD_STOWED; + maxTargetPos = climbConstants.CLIMB_OUT_MAX_PRESET; + minTargetPos = climbConstants.CLIMB_OUT_MIN_PRESET; + holdSpeed = climbConstants.CLIMB_HOLD_STOWED; moveComplete = false; } case CLIMBED -> { // if climbed, it sets the pos its going to as climbed, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMBED; - maxTargetPos = CLIMBED_MAX_PRESET; - minTargetPos = CLIMBED_MIN_PRESET; - holdSpeed = CLIMB_HOLD_CLIMBOUT; + maxTargetPos = climbConstants.CLIMBED_MAX_PRESET; + minTargetPos = climbConstants.CLIMBED_MIN_PRESET; + holdSpeed = climbConstants.CLIMB_HOLD_CLIMBOUT; moveComplete = false; } } @@ -331,12 +307,14 @@ public void periodic() { double currentPos = getClimbPosition(); // checks what state its in position wise and returns if its in that state - if (CLIMB_OUT_MIN_PRESET <= currentPos && currentPos <= CLIMB_OUT_MAX_PRESET) { + if (climbConstants.CLIMB_OUT_MIN_PRESET <= currentPos + && currentPos <= climbConstants.CLIMB_OUT_MAX_PRESET) { isClimbOut = true; } else { isClimbOut = false; } - if (STOWED_MIN_PRESET <= currentPos && currentPos <= STOWED_MAX_PRESET) { + if (climbConstants.STOWED_MIN_PRESET <= currentPos + && currentPos <= climbConstants.STOWED_MAX_PRESET) { isStowed = true; } else { isStowed = false; @@ -397,11 +375,11 @@ public Command advanceClimbCheck() { if (!moveComplete) { pError = minTargetPos - getClimbPosition(); if (pError > 0) { - speedToSet = CLIMB_OUT_SPEED * pError * climbOutKp; + speedToSet = climbConstants.CLIMB_OUT_SPEED * pError * climbConstants.climbOutKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } else { - speedToSet = CLIMB_IN_SPEED * -pError * climbInKp; + speedToSet = climbConstants.CLIMB_IN_SPEED * -pError * climbConstants.climbInKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 371953a9..9a43e912 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -23,48 +23,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Hardware; +import frc.robot.Constants.elevatorConstants; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; public class ElevatorSubsystem extends SubsystemBase { - // Maximum is 38.34 - public static final double CORAL_LEVEL_FOUR_PRE_POS = 37.5; - public static final double CORAL_LEVEL_FOUR_POS = 36; - public static final double CORAL_LEVEL_THREE_PRE_POS = 18.65; - public static final double CORAL_LEVEL_THREE_POS = 14; - public static final double CORAL_LEVEL_TWO_PRE_POS = 6.94; - public static final double CORAL_LEVEL_TWO_POS = 4.4; - public static final double CORAL_LEVEL_ONE_POS = 4.2; - public static final double ALGAE_LEVEL_TWO_THREE = 11; - public static final double ALGAE_LEVEL_TWO_THREE_FLING = 16; - public static final double ALGAE_LEVEL_THREE_FOUR = 21; - public static final double ALGAE_LEVEL_THREE_FOUR_FLING = 25; - public static final double ALGAE_STOWED = 2; - public static final double ALGAE_PROCESSOR_SCORE = 2; - public static final double ALGAE_NET_SCORE = 38; // untested - public static final double ALGAE_GROUND_INTAKE = 0.05; - public static final double CORAL_STOWED = CORAL_LEVEL_TWO_PRE_POS; - public static final double CORAL_GROUND_INTAKE_POS = 7.2; - public static final double CORAL_INTAKE_POS = 1.55; - public static final double CORAL_PRE_INTAKE = 4.7; - public static final double CORAL_QUICK_INTAKE = 1.6; - public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; - public static final double MIN_FULL_GROUND_INTAKE = 8.0; - public static final double MANUAL = 0.1; - private static final double POS_TOLERANCE = 0.1; - private final double ELEVATOR_KP = 7.804; - private final double ELEVATOR_KI = 0; - private final double ELEVATOR_KD = 0.079221; - private final double ELEVATOR_KS = 0.33878; - private final double ELEVATOR_KV = 0.12975; - private final double ELEVATOR_KA = 0.0070325; - private final double REVERSE_SOFT_LIMIT = -0.05; - private final double FORWARD_SOFT_LIMIT = 38; - public static final double UP_VOLTAGE = 5; - private final double DOWN_VOLTAGE = -3; - private final double HOLD_VOLTAGE = 0.6; // create a Motion Magic request, voltage output private final MotionMagicVoltage m_request = new MotionMagicVoltage(0); @@ -100,8 +64,8 @@ public class ElevatorSubsystem extends SubsystemBase { /** Subsystem constructor. */ public ElevatorSubsystem() { // m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse); - m_motor = new TalonFX(Hardware.ELEVATOR_MOTOR_ONE, "Drivebase"); - m_motor2 = new TalonFX(Hardware.ELEVATOR_MOTOR_TWO, "Drivebase"); + m_motor = new TalonFX(elevatorConstants.ELEVATOR_MOTOR_ONE, "Drivebase"); + m_motor2 = new TalonFX(elevatorConstants.ELEVATOR_MOTOR_TWO, "Drivebase"); m_motorOneSimState = m_motor.getSimState(); m_motorTwoSimState = m_motor2.getSimState(); motorConfigs(); @@ -170,9 +134,9 @@ public void motorConfigs() { TalonFXConfiguration configuration = new TalonFXConfiguration(); // enable stator current limit - configuration.CurrentLimits.StatorCurrentLimit = 160; + configuration.CurrentLimits.StatorCurrentLimit = elevatorConstants.CURRENT_STATOR_LIMIT; configuration.CurrentLimits.StatorCurrentLimitEnable = true; - configuration.CurrentLimits.SupplyCurrentLimit = 80; + configuration.CurrentLimits.SupplyCurrentLimit = elevatorConstants.CURRENT_SUPPLY_LIMIT; configuration.CurrentLimits.SupplyCurrentLimitEnable = true; // create brake mode for motors @@ -182,12 +146,12 @@ public void motorConfigs() { talonFXConfigurator2.apply(configuration); // set slot 0 gains - configuration.Slot0.kS = ELEVATOR_KS; - configuration.Slot0.kV = ELEVATOR_KV; - configuration.Slot0.kA = ELEVATOR_KA; - configuration.Slot0.kP = ELEVATOR_KP; - configuration.Slot0.kI = ELEVATOR_KI; - configuration.Slot0.kD = ELEVATOR_KD; + configuration.Slot0.kS = elevatorConstants.ELEVATOR_KS; + configuration.Slot0.kV = elevatorConstants.ELEVATOR_KV; + configuration.Slot0.kA = elevatorConstants.ELEVATOR_KA; + configuration.Slot0.kP = elevatorConstants.ELEVATOR_KP; + configuration.Slot0.kI = elevatorConstants.ELEVATOR_KI; + configuration.Slot0.kD = elevatorConstants.ELEVATOR_KD; // set Motion Magic settings // Bottom to full: ~40 rotations @@ -226,7 +190,7 @@ public void setRumble(DoubleConsumer rumble) { } public boolean atPosition(double position) { - return MathUtil.isNear(position, getCurrentPosition(), POS_TOLERANCE); + return MathUtil.isNear(position, getCurrentPosition(), elevatorConstants.POS_TOLERANCE); } public boolean getHasBeenZeroed() { @@ -254,7 +218,7 @@ private void setCurrentPosition(double pos) { } public Trigger above(double position) { - return new Trigger(() -> getCurrentPosition() >= position - POS_TOLERANCE); + return new Trigger(() -> getCurrentPosition() >= position - elevatorConstants.POS_TOLERANCE); } public Command resetPosZero() { @@ -301,18 +265,18 @@ public Command holdCoastMode() { } public Command goUp() { - return defer(() -> setLevel(getCurrentPosition() + MANUAL)); + return defer(() -> setLevel(getCurrentPosition() + elevatorConstants.MANUAL)); } public Command goDown() { - return defer(() -> setLevel(getCurrentPosition() - MANUAL)); + return defer(() -> setLevel(getCurrentPosition() - elevatorConstants.MANUAL)); } public Command goUpPower(DoubleSupplier scale) { return runEnd( () -> { - m_motor.setVoltage(scale.getAsDouble() * UP_VOLTAGE); - m_motor2.setVoltage(scale.getAsDouble() * -UP_VOLTAGE); + m_motor.setVoltage(scale.getAsDouble() * elevatorConstants.UP_VOLTAGE); + m_motor2.setVoltage(scale.getAsDouble() * -elevatorConstants.UP_VOLTAGE); }, () -> { m_motor.stopMotor(); @@ -326,8 +290,8 @@ public Command goUpPower(DoubleSupplier scale) { public Command goDownPower(DoubleSupplier scale) { return startEnd( () -> { - m_motor.setVoltage(scale.getAsDouble() * DOWN_VOLTAGE); - m_motor2.setVoltage(scale.getAsDouble() * -DOWN_VOLTAGE); + m_motor.setVoltage(scale.getAsDouble() * elevatorConstants.DOWN_VOLTAGE); + m_motor2.setVoltage(scale.getAsDouble() * -elevatorConstants.DOWN_VOLTAGE); }, () -> { m_motor.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 56f1d3e1..41ca8a84 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -9,31 +9,14 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.groundArmConstants; public class GroundArm extends SubsystemBase { - private final double ARMPIVOT_KP = 40; - private final double ARMPIVOT_KI = 0; - private final double ARMPIVOT_KD = 0; - private final double ARMPIVOT_KS = 0.9; - private final double ARMPIVOT_KV = 4; - private final double ARMPIVOT_KG = 0.048; - private final double ARMPIVOT_KA = 0; - public static final double STOWED_POSITION = 0.46; - public static final double UP_POSITION = - 0.27; // untested - should be somewhere in between stowed and ground - public static final double GROUND_POSITION = -0.050; - public static final double QUICK_INTAKE_POSITION = 0.31; - public static final double POS_TOLERANCE = Units.degreesToRotations(5); - public static final double GROUND_HOLD_VOLTAGE = -0.40; - // ratio from motor rotations to output rotations - private static final double ARM_RATIO = 60; // MotionMagic voltage private final MotionMagicVoltage m_request = new MotionMagicVoltage(0); @@ -45,7 +28,7 @@ public class GroundArm extends SubsystemBase { private double targetPos; public GroundArm() { - motor = new TalonFX(Hardware.GROUND_INTAKE_ARM_MOTOR); + motor = new TalonFX(groundArmConstants.GROUND_INTAKE_ARM_MOTOR); configMotors(); logTabs(); } @@ -55,9 +38,10 @@ public void configMotors() { TalonFXConfigurator cfg = motor.getConfigurator(); var talonFXConfiguration = new TalonFXConfiguration(); - talonFXConfiguration.Feedback.FeedbackRemoteSensorID = Hardware.GROUND_INTAKE_ARM_ENCODER; + talonFXConfiguration.Feedback.FeedbackRemoteSensorID = + groundArmConstants.GROUND_INTAKE_ARM_ENCODER; talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - talonFXConfiguration.Feedback.RotorToSensorRatio = ARM_RATIO; + talonFXConfiguration.Feedback.RotorToSensorRatio = groundArmConstants.ARM_RATIO; talonFXConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -70,13 +54,13 @@ public void configMotors() { // PID // set slot 0 gains - talonFXConfiguration.Slot0.kS = ARMPIVOT_KS; - talonFXConfiguration.Slot0.kV = ARMPIVOT_KV; - talonFXConfiguration.Slot0.kA = ARMPIVOT_KA; - talonFXConfiguration.Slot0.kP = ARMPIVOT_KP; - talonFXConfiguration.Slot0.kI = ARMPIVOT_KI; - talonFXConfiguration.Slot0.kD = ARMPIVOT_KD; - talonFXConfiguration.Slot0.kG = ARMPIVOT_KG; + talonFXConfiguration.Slot0.kS = groundArmConstants.ARMPIVOT_KS; + talonFXConfiguration.Slot0.kV = groundArmConstants.ARMPIVOT_KV; + talonFXConfiguration.Slot0.kA = groundArmConstants.ARMPIVOT_KA; + talonFXConfiguration.Slot0.kP = groundArmConstants.ARMPIVOT_KP; + talonFXConfiguration.Slot0.kI = groundArmConstants.ARMPIVOT_KI; + talonFXConfiguration.Slot0.kD = groundArmConstants.ARMPIVOT_KD; + talonFXConfiguration.Slot0.kG = groundArmConstants.ARMPIVOT_KG; talonFXConfiguration.Slot0.GravityType = GravityTypeValue.Arm_Cosine; // set Motion Magic settings in motor rps not mechanism units @@ -127,7 +111,8 @@ public Command setVoltage(double voltage) { } public Trigger atPosition(double position) { - return new Trigger(() -> Math.abs(getCurrentPosition() - position) < POS_TOLERANCE); + return new Trigger( + () -> Math.abs(getCurrentPosition() - position) < groundArmConstants.POS_TOLERANCE); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 9398d571..7fc5705c 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -9,14 +9,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Hardware; +import frc.robot.Constants.groundSpinnyConstants; public class GroundSpinny extends SubsystemBase { - public static final double GROUND_INTAKE_SPEED = -8; - public static final double GROUND_INTAKE_JITTER_SPEED = 1; - public static final double FUNNEL_INTAKE_SPEED = -2; - public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; - private static final double STATOR_CURRENT_STALL_THRESHOLD = 50; // TalonFX private final TalonFX motor; @@ -35,7 +30,7 @@ public void logTabs() { } public GroundSpinny() { - motor = new TalonFX(Hardware.GROUND_INTAKE_SPINNY_MOTOR); + motor = new TalonFX(groundSpinnyConstants.GROUND_INTAKE_SPINNY_MOTOR); configMotors(); logTabs(); } @@ -73,45 +68,53 @@ private Command holdPower(double pow) { } public Command setFunnelIntakePower() { - return setPower(FUNNEL_INTAKE_SPEED).withName("set funnel intake power"); + return setPower(groundSpinnyConstants.FUNNEL_INTAKE_SPEED).withName("set funnel intake power"); } public Command holdFunnelIntakePower() { - return holdPower(FUNNEL_INTAKE_SPEED).withName("hold funnel intake power"); + return holdPower(groundSpinnyConstants.FUNNEL_INTAKE_SPEED) + .withName("hold funnel intake power"); } public void imperativeSetGroundIntakePower() { - motor.setVoltage(GROUND_INTAKE_SPEED); - lastSetPower = GROUND_INTAKE_SPEED; + motor.setVoltage(groundSpinnyConstants.GROUND_INTAKE_SPEED); + lastSetPower = groundSpinnyConstants.GROUND_INTAKE_SPEED; } public Command setGroundIntakePower() { - return setPower(GROUND_INTAKE_SPEED).withName("set ground intake power"); + return setPower(groundSpinnyConstants.GROUND_INTAKE_SPEED).withName("set ground intake power"); } public Command holdGroundIntakePower() { - return holdPower(GROUND_INTAKE_SPEED).withName("hold ground intake power"); + return holdPower(groundSpinnyConstants.GROUND_INTAKE_SPEED) + .withName("hold ground intake power"); } public Command setQuickHandoffExtakeSpeed() { - return setPower(QUICK_HANDOFF_EXTAKE_SPEED).withName("set quick handoff extake power"); + return setPower(groundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) + .withName("set quick handoff extake power"); } public Command holdQuickHandoffExtakeSpeed() { - return holdPower(QUICK_HANDOFF_EXTAKE_SPEED).withName("hold quick handoff extake power"); + return holdPower(groundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) + .withName("hold quick handoff extake power"); } public Command setGroundIntakeJitterSpeed() { - return setPower(GROUND_INTAKE_JITTER_SPEED).withName("set ground intake jitter power"); + return setPower(groundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) + .withName("set ground intake jitter power"); } public Command holdGroundIntakeJitterSpeed() { - return holdPower(GROUND_INTAKE_JITTER_SPEED).withName("hold ground intake jitter power"); + return holdPower(groundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) + .withName("hold ground intake jitter power"); } public Trigger stalling() { return new Trigger( - () -> motor.getStatorCurrent().getValueAsDouble() > STATOR_CURRENT_STALL_THRESHOLD); + () -> + motor.getStatorCurrent().getValueAsDouble() + > groundSpinnyConstants.STATOR_CURRENT_STALL_THRESHOLD); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..e3a9dddc 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -15,21 +15,12 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Hardware; +import frc.robot.Constants.spinnyClawConstants; import frc.robot.sensors.ArmSensor; import frc.robot.util.ScoringMode; import java.util.function.Supplier; public class SpinnyClaw extends SubsystemBase { - public static final double CORAL_INTAKE_SPEED = -6; - public static final double CORAL_EXTAKE_SPEED = 5; - public static final double CORAL_REJECT_SPEED = 1; - public static final double CORAL_L1_EXTAKE_SPEED = 1.7; - public static final double ALGAE_INTAKE_SPEED = -4; // started at -3.5 - public static final double ALGAE_GRIP_INTAKE_SPEED = -3; // started at -2.5 - public static final double ALGAE_EXTAKE_SPEED = 14; - public static final double ALGAE_PROCESSOR_EXTAKE_SPEED = 8; - public static final double ALGAE_FLING_SPEED = 10; // Remove once we implement PID speed public static int placeholderPIDSpeed; @@ -46,7 +37,7 @@ public class SpinnyClaw extends SubsystemBase { private double lastSetPower; public SpinnyClaw(ArmSensor armSensor) { - motor = new TalonFX(Hardware.SPINNY_CLAW_MOTOR_ID); + motor = new TalonFX(spinnyClawConstants.SPINNY_CLAW_MOTOR_ID); this.armSensor = armSensor; configMotors(); logTabs(); @@ -132,53 +123,56 @@ private Command holdPower(double pow) { } public Command coralIntakePower() { - return setPower(CORAL_INTAKE_SPEED).withName("Intake power"); + return setPower(spinnyClawConstants.CORAL_INTAKE_SPEED).withName("Intake power"); } public Command coralExtakePower() { - return setPower(CORAL_EXTAKE_SPEED).withName("Extake power"); + return setPower(spinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Extake power"); } public Command coralRejectPower() { - return setPower(CORAL_REJECT_SPEED).withName("Extake power"); + return setPower(spinnyClawConstants.CORAL_REJECT_SPEED).withName("Extake power"); } public Command coralHoldIntakePower() { - return holdPower(CORAL_INTAKE_SPEED).withName("Hold intake power"); + return holdPower(spinnyClawConstants.CORAL_INTAKE_SPEED).withName("Hold intake power"); } public Command coralHoldExtakePower() { - return holdPower(CORAL_EXTAKE_SPEED).withName("Hold extake power"); + return holdPower(spinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Hold extake power"); } public Command coralLevelOneHoldExtakePower() { - return holdPower(CORAL_L1_EXTAKE_SPEED).withName("Level 1 hold extake power"); + return holdPower(spinnyClawConstants.CORAL_L1_EXTAKE_SPEED) + .withName("Level 1 hold extake power"); } // algae stuff public Command algaeIntakePower() { - return setPower(ALGAE_INTAKE_SPEED).withName("Algae intake power"); + return setPower(spinnyClawConstants.ALGAE_INTAKE_SPEED).withName("Algae intake power"); } public Command algaeExtakePower() { // can change to net extake maybe? also bound as general extake though - return setPower(ALGAE_EXTAKE_SPEED).withName("Algae extake power"); + return setPower(spinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae extake power"); } public Command algaeGripIntakePower() { - return setPower(ALGAE_GRIP_INTAKE_SPEED).withName("Algae grip intake power"); + return setPower(spinnyClawConstants.ALGAE_GRIP_INTAKE_SPEED) + .withName("Algae grip intake power"); } public Command algaeHoldExtakePower() { - return holdPower(ALGAE_EXTAKE_SPEED).withName("Algae hold extake power"); + return holdPower(spinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae hold extake power"); } public Command algaeExtakeProcessorPower() { - return setPower(ALGAE_PROCESSOR_EXTAKE_SPEED).withName("Algae processor extake power"); + return setPower(spinnyClawConstants.ALGAE_PROCESSOR_EXTAKE_SPEED) + .withName("Algae processor extake power"); } public Command algaeFlingPower() { - return setPower(ALGAE_FLING_SPEED).withName("Algae fling power"); + return setPower(spinnyClawConstants.ALGAE_FLING_SPEED).withName("Algae fling power"); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 41ec29d6..767a9833 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -4,6 +4,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.armPivotConstants; +import frc.robot.Constants.elevatorConstants; +import frc.robot.Constants.groundArmConstants; import frc.robot.sensors.ArmSensor; import frc.robot.sensors.BranchSensors; import frc.robot.sensors.ElevatorLight; @@ -88,24 +91,26 @@ public Command coralLevelFour(BooleanSupplier score) { Commands.parallel( Commands.print("Pre position"), elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_FOUR_PRE_POS) .deadlineFor( // keeps spinny claw engaged until coral has been scored - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), + armPivot + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .until(score)), spinnyClaw.stop()) .withTimeout(0.7), repeatPrescoreScoreSwing( Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_FOUR_PRE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_PRE_L4)) + elevator.setLevel(elevatorConstants.CORAL_LEVEL_FOUR_PRE_POS), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_PRE_L4)) .withDeadline( Commands.waitUntil( score)), // waits until driver presses the score button or until // auto scoring happens armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), Commands.print("Pre preIntake()"), coralPreIntake() @@ -122,20 +127,22 @@ public Command coralLevelThree(BooleanSupplier score) { // same as L4 return Commands.sequence( Commands.parallel( elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_THREE_PRE_POS) .deadlineFor( - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), + armPivot + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .until(score)), spinnyClaw.stop()) .withTimeout(0.5), repeatPrescoreScoreSwing( Commands.repeatingSequence( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) .withDeadline(Commands.waitUntil(score)), armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) @@ -146,20 +153,22 @@ public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 return Commands.sequence( Commands.parallel( elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS) .deadlineFor( - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP).until(score)), + armPivot + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .until(score)), spinnyClaw.stop()) .withTimeout(0.5), repeatPrescoreScoreSwing( Commands.sequence( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L2) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L2) .withDeadline(Commands.waitUntil(score)), armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) @@ -169,8 +178,8 @@ public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 public Command coralLevelOne(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), + elevator.setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L1), spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(score)), @@ -190,10 +199,10 @@ public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { return Commands.sequence( Commands.parallel( elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_THREE_PRE_POS) .deadlineFor( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(ipScore) .until(score)), spinnyClaw.stop()) @@ -201,12 +210,12 @@ public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { repeatPrescoreScoreSwing( Commands.repeatingSequence( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) .withDeadline(Commands.waitUntil(ipScore).until(score)), armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score, ipScore), coralPreIntake()) @@ -219,10 +228,10 @@ public Command coralLevelTwo( return Commands.sequence( Commands.parallel( elevator - .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) + .setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS) .deadlineFor( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(ipScore) .until(score)), spinnyClaw.stop()) @@ -230,12 +239,12 @@ public Command coralLevelTwo( repeatPrescoreScoreSwing( Commands.sequence( armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_L2) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L2) .withDeadline(Commands.waitUntil(ipScore).until(score)), armPivot - .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score, ipScore), coralPreIntake()) @@ -246,8 +255,8 @@ public Command coralLevelTwo( public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), + elevator.setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L1), spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), @@ -270,24 +279,25 @@ public Command groundIntake(BooleanSupplier retract) { BooleanSupplier clawFull = armSensor != null ? armSensor.inClaw() : () -> false; return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_GROUND_INTAKE_POS), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_GROUND_INTAKE), + elevator.setLevel(elevatorConstants.CORAL_GROUND_INTAKE_POS), + armPivot.moveToPosition( + armPivotConstants.ARMPIVOT_CORAL_PRESET_GROUND_INTAKE), spinnyClaw.stop(), // just as a backup in case things are silly groundSpinny.setGroundIntakePower()) .until( elevator.above( - ElevatorSubsystem + elevatorConstants .MIN_EMPTY_GROUND_INTAKE)), // waits until elevator is out of the way // to lift ground intake groundArm - .moveToPosition(GroundArm.GROUND_POSITION) - .andThen(groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE)) + .moveToPosition(groundArmConstants.GROUND_POSITION) + .andThen(groundArm.setVoltage(groundArmConstants.GROUND_HOLD_VOLTAGE)) .withDeadline( Commands.waitUntil( intakeSensor .inIntake() .or(retract))), // ends ground pickup to stow coral for scoring - groundArm.moveToPosition(GroundArm.STOWED_POSITION), + groundArm.moveToPosition(groundArmConstants.STOWED_POSITION), groundSpinny.setFunnelIntakePower(), coralPreIntake()) .unless(clawFull) @@ -310,26 +320,26 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Initial setup- Move elevator high enough for ground arm to be clear, start moving // arm pivot, and start spinning the ground intake Commands.parallel( - elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + elevator.setLevel(elevatorConstants.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough - .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), + .until(elevator.above(elevatorConstants.MIN_EMPTY_GROUND_INTAKE)), // Core intake sequence Commands.sequence( // Deploy the ground arm (and wait until it reaches the position). - groundArm.moveToPosition(GroundArm.GROUND_POSITION), + groundArm.moveToPosition(groundArmConstants.GROUND_POSITION), // After it's deployed, apply a constant voltage to press it into the bumper // and continue. - groundArm.setVoltage(GroundArm.GROUND_HOLD_VOLTAGE), + groundArm.setVoltage(groundArmConstants.GROUND_HOLD_VOLTAGE), // Grabbing segment Commands.parallel( // These three are the initial setup: Move elevator down to the handoff // height, make sure armPivot finishes moving to the right height, and // spin claw - elevator.setLevel(ElevatorSubsystem.CORAL_QUICK_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + elevator.setLevel(elevatorConstants.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), // Run the intake with occassional jitters. (This is stopped when the core // intake sequence is stopped.) @@ -355,16 +365,16 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Retract the groundArm all the way to stowed (so that we aren't slowing down to stop // in the middle), but move on to the next command when we're at the handoff point. groundArm - .moveToPosition(GroundArm.STOWED_POSITION) - .until(groundArm.atPosition(GroundArm.QUICK_INTAKE_POSITION)), + .moveToPosition(groundArmConstants.STOWED_POSITION) + .until(groundArm.atPosition(groundArmConstants.QUICK_INTAKE_POSITION)), // Spin groundSpinny out, but skip if we lost the coral. groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), // Go back to stow, but skip if we lost the coral. coralStow().onlyIf(armSensor.inClaw()), // If we lost the coral, reset for a quick retry. Commands.parallel( - elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), + elevator.setLevel(elevatorConstants.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.stop()) .onlyIf(armSensor.inClaw().negate())) // Don't run the entire sequence at all if the claw is full. @@ -379,23 +389,23 @@ public Command coralStow() { BranchHeight currentBranchHeight = branchHeight.get(); double elevatorHeight = switch (currentBranchHeight) { // used to stow closer to the scoring height - case CORAL_LEVEL_FOUR -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS; - case CORAL_LEVEL_THREE -> ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS; - case CORAL_LEVEL_TWO -> ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS; - case CORAL_LEVEL_ONE -> ElevatorSubsystem.CORAL_LEVEL_ONE_POS; + case CORAL_LEVEL_FOUR -> elevatorConstants.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_THREE -> elevatorConstants.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_TWO -> elevatorConstants.CORAL_LEVEL_TWO_PRE_POS; + case CORAL_LEVEL_ONE -> elevatorConstants.CORAL_LEVEL_ONE_POS; }; - if (elevatorHeight > ElevatorSubsystem.CORAL_PRE_INTAKE) { + if (elevatorHeight > elevatorConstants.CORAL_PRE_INTAKE) { return Commands.parallel( elevator.setLevel(elevatorHeight), Commands.sequence( // waits to raise the arm until elevator is above preintake - Commands.waitUntil(elevator.above(ElevatorSubsystem.CORAL_PRE_INTAKE)), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)), + Commands.waitUntil(elevator.above(elevatorConstants.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), spinnyClaw.stop()); } else { return Commands.parallel( Commands.sequence( - elevator.setLevel(ElevatorSubsystem.CORAL_PRE_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP), + elevator.setLevel(elevatorConstants.CORAL_PRE_INTAKE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP), elevator.setLevel(elevatorHeight)), spinnyClaw.stop()); } @@ -408,10 +418,10 @@ public Command autoCoralStow() { return Commands.defer( () -> Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS), + elevator.setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS), Commands.sequence( - Commands.waitUntil(elevator.above(ElevatorSubsystem.CORAL_PRE_INTAKE)), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)), + Commands.waitUntil(elevator.above(elevatorConstants.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), spinnyClaw.stop()), Set.of(elevator, armPivot, spinnyClaw)) .withName("Coral Stow"); @@ -419,8 +429,8 @@ public Command autoCoralStow() { public Command coralPreIntake() { return Commands.parallel( - elevator.setLevel(ElevatorSubsystem.CORAL_PRE_INTAKE), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN), + elevator.setLevel(elevatorConstants.CORAL_PRE_INTAKE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN), spinnyClaw.coralRejectPower()) // keeps coral out while waiting to intake .andThen(Commands.print("end of preIntake()")) .withName("PreIntake"); @@ -431,8 +441,8 @@ public Command coralPreIntake() { // in preintake position return new Trigger( () -> - elevator.atPosition(ElevatorSubsystem.CORAL_PRE_INTAKE) - && armPivot.atPosition(ArmPivot.CORAL_PRESET_DOWN)); + elevator.atPosition(elevatorConstants.CORAL_PRE_INTAKE) + && armPivot.atPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)); } public Command coralIntake() { // yummy coral @@ -440,8 +450,8 @@ public Command coralIntake() { // yummy coral Commands.sequence( Commands.parallel( spinnyClaw.coralIntakePower(), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)), - elevator.setLevel(ElevatorSubsystem.CORAL_INTAKE_POS)), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), + elevator.setLevel(elevatorConstants.CORAL_INTAKE_POS)), coralStow()) .withName("Coral Intake"); } @@ -451,8 +461,8 @@ public Command autoCoralIntake() { // yummy coral Commands.sequence( Commands.parallel( spinnyClaw.coralIntakePower(), - armPivot.moveToPosition(ArmPivot.CORAL_PRESET_DOWN)), - elevator.setLevel(ElevatorSubsystem.CORAL_INTAKE_POS)), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), + elevator.setLevel(elevatorConstants.CORAL_INTAKE_POS)), autoCoralStow()) .withName("Coral Intake"); } @@ -461,8 +471,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), - armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR))) + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE), + elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR))) .withName("Algae L3-L4 Intake"); } @@ -470,8 +480,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), - armPivot.moveToPosition(ArmPivot.ALGAE_REMOVE), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE))) + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE), + elevator.setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE))) .withName("Algae L2-L3 Intake"); } @@ -480,15 +490,15 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( - armPivot.moveToPosition(ArmPivot.ALGAE_GROUND_INTAKE), - elevator.setLevel(ElevatorSubsystem.ALGAE_GROUND_INTAKE))) + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_GROUND_INTAKE), + elevator.setLevel(elevatorConstants.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } public Command algaeStow() { // holds algae return Commands.parallel( - elevator.setLevel(ElevatorSubsystem.ALGAE_STOWED), - armPivot.moveToPosition(ArmPivot.ALGAE_STOWED), + elevator.setLevel(elevatorConstants.ALGAE_STOWED), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_STOWED), spinnyClaw.algaeGripIntakePower()) .deadlineFor(colorSet(255, 255, 255, "White - Stowed").asProxy()) .withName("Algae Stow"); @@ -499,8 +509,8 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p Commands.parallel( spinnyClaw.algaeGripIntakePower(), Commands.sequence( - armPivot.moveToPosition(ArmPivot.ALGAE_PROCESSOR_SCORE), - elevator.setLevel(ElevatorSubsystem.ALGAE_PROCESSOR_SCORE))), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE), + elevator.setLevel(elevatorConstants.ALGAE_PROCESSOR_SCORE))), Commands.waitUntil(score), spinnyClaw.algaeExtakeProcessorPower()) .withName("Algae Processor Score"); @@ -509,15 +519,17 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), - armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), + elevator.setLevel(elevatorConstants.ALGAE_NET_SCORE), + armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), Commands.waitSeconds(0.7), armPivot.moveToPosition( - ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) + armPivotConstants + .ARMPIVOT_CORAL_PRESET_UP), // added to prevent hitting the barge after scoring + // net + elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae Net Score"); } @@ -526,7 +538,7 @@ public Command algaeNetScore(BooleanSupplier score) { Commands.parallel( spinnyClaw.algaeFlingPower(), armPivot.moveToPosition(ArmPivot.ALGAE_FLING), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR_FLING)), + elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR_FLING)), Commands.waitUntil(finish), algaeStow()) .withName("Algae L3-L4 Fling"); @@ -537,7 +549,7 @@ public Command algaeLevelTwoThreeFling(BooleanSupplier finish) { Commands.parallel( spinnyClaw.algaeFlingPower(), armPivot.moveToPosition(ArmPivot.ALGAE_FLING), - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE_FLING)), + elevator.setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE_FLING)), Commands.waitUntil(finish), algaeStow()) .withName("Algae L2-L3 Fling"); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 43e8f322..3ef06e3f 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -3,14 +3,9 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; 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.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; @@ -24,7 +19,7 @@ import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Hardware; +import frc.robot.Constants.visionConstants_Misc; import java.util.Optional; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -38,43 +33,6 @@ * station (+X is forward from blue driver station, +Y is left, +Z is up). */ public class VisionSubsystem extends SubsystemBase { - // differences from center robot camera poses - private static final double CAMERA_X_POS_METERS_LEFT = 0.26; - private static final double CAMERA_X_POS_METERS_RIGHT = 0.27; - private static final double CAMERA_Y_POS_METERS_LEFT = 0.25; - private static final double CAMERA_Y_POS_METERS_RIGHT = -0.25; - private static final double CAMERA_Z_POS_METERS_LEFT = 0.20; - private static final double CAMERA_Z_POS_METERS_RIGHT = 0.21; - private static final double CAMERA_ROLL_LEFT = Units.degreesToRadians(3); - private static final double CAMERA_ROLL_RIGHT = Units.degreesToRadians(0.92); - private static final double CAMERA_PITCH_LEFT = Units.degreesToRadians(-6.3); - private static final double CAMERA_PITCH_RIGHT = Units.degreesToRadians(-8.3); - private static final double CAMERA_YAW_LEFT = Units.degreesToRadians(-44.64); - private static final double CAMERA_YAW_RIGHT = Units.degreesToRadians(46.42); - // left camera diffrences from center robot - public static final Transform3d ROBOT_TO_CAM_LEFT = - new Transform3d( - // Translation3d.kZero, - CAMERA_X_POS_METERS_LEFT, - CAMERA_Y_POS_METERS_LEFT, - CAMERA_Z_POS_METERS_LEFT, - // Rotation3d.kZero); - new Rotation3d(CAMERA_ROLL_LEFT, CAMERA_PITCH_LEFT, CAMERA_YAW_LEFT)); - // right camera diffrences from center robot - public static final Transform3d ROBOT_TO_CAM_RIGHT = - new Transform3d( - // Translation3d.kZero, - CAMERA_X_POS_METERS_RIGHT, - CAMERA_Y_POS_METERS_RIGHT, - CAMERA_Z_POS_METERS_RIGHT, - // Rotation3d.kZero); - new Rotation3d(CAMERA_ROLL_RIGHT, CAMERA_PITCH_RIGHT, CAMERA_YAW_RIGHT)); - - // Deviations - private static final Vector STANDARD_DEVS = - VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(20)); - private static final Vector DISTANCE_SC_STANDARD_DEVS = - VecBuilder.fill(1, 1, Units.degreesToRadians(50)); // making the cameras, pose estimator, field2d, fieldObject2d, april tags helper objects private final PhotonCamera leftCamera; @@ -126,17 +84,21 @@ public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) { this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = robotField.getObject("RawVision"); // cameras init hardware wise - leftCamera = new PhotonCamera(Hardware.LEFT_CAM); - rightCamera = new PhotonCamera(Hardware.RIGHT_CAM); + leftCamera = new PhotonCamera(visionConstants_Misc.LEFT_CAM); + rightCamera = new PhotonCamera(visionConstants_Misc.RIGHT_CAM); // pose estimator inits for cameras with full field, multi-tag april tag detection and camera // differences from center robot // pose estimator is used to estimate the robot's position on the field based on the cameras photonPoseEstimatorLeftCamera = new PhotonPoseEstimator( - fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_LEFT); + fieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + visionConstants_Misc.ROBOT_TO_CAM_LEFT); photonPoseEstimatorRightCamera = new PhotonPoseEstimator( - fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, ROBOT_TO_CAM_RIGHT); + fieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + visionConstants_Misc.ROBOT_TO_CAM_RIGHT); // vision shuffle board tab creation ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags"); @@ -234,7 +196,10 @@ private void process( TimestampSeconds, // start with STANDARD_DEVS, and for every meter of distance past 1 meter, add another // DISTANCE_SC_STANDARD_DEVS to the standard devs - DISTANCE_SC_STANDARD_DEVS.times(Math.max(0, Distance - 1)).plus(STANDARD_DEVS)); + visionConstants_Misc + .DISTANCE_SC_STANDARD_DEVS + .times(Math.max(0, Distance - 1)) + .plus(visionConstants_Misc.STANDARD_DEVS)); // sets estimated current pose to estimated vision pose robotField.setRobotPose(aprilTagsHelper.getEstimatedPosition()); // updates shuffleboard values From 259ac16e29451eab3c785c2df4950ca9806682b1 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:42:07 -0800 Subject: [PATCH 02/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c4949a47..94dfd23c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -169,7 +169,7 @@ public class groundSpinnyConstants { public static final int GROUND_INTAKE_SPINNY_MOTOR = 46; } - public class spinnyClawConstants { + public static class SpinnyClawConstants { public static final double CORAL_INTAKE_SPEED = -6; public static final double CORAL_EXTAKE_SPEED = 5; public static final double CORAL_REJECT_SPEED = 1; From 19942cd3c91f5aa3e923eac289e98642aa32a643 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:42:54 -0800 Subject: [PATCH 03/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 94dfd23c..f42ff154 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,7 +53,7 @@ public class armPivotConstants { public static final int MAIN_ARM_SENSOR = 35; } - public class climbConstants { + public static class ClimbConstants { // various position and speed presets public static final double STOWED_MAX_PRESET = -0.447; public static final double STOWED_MIN_PRESET = -0.450; From 26f159d7d01b98bfe810881041cbd544a4a15b3a Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:43:03 -0800 Subject: [PATCH 04/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f42ff154..13c5f068 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,7 +8,7 @@ import edu.wpi.first.math.util.Units; public class Constants { - public class armPivotConstants { + public static class ArmPivotConstants { // Presets public static final double ARMPIVOT_KP = 38.5; // previously 50 public static final double ARMPIVOT_KI = 0; From deb6fa33acf39407fabd09dc7bee8dd667e76bdd Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:43:36 -0800 Subject: [PATCH 05/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 13c5f068..3bc7eda1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,7 +90,7 @@ public static class ClimbConstants { public static final int CLIMB_PIVOT_CANCODER_ID = 52; } - public class elevatorConstants { + public static class ElevatorConstants { // Maximum is 38.34 public static final double CORAL_LEVEL_FOUR_PRE_POS = 37.5; public static final double CORAL_LEVEL_FOUR_POS = 36; From 95c09ca1357fb9c1229bfe450e71ae7d30fda154 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:43:57 -0800 Subject: [PATCH 06/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3bc7eda1..6ca35a33 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -137,7 +137,7 @@ public static class ElevatorConstants { public static final int ELEVATOR_MOTOR_TWO = 21; } - public class groundArmConstants { + public static class GroundArmConstants { public static final double ARMPIVOT_KP = 40; public static final double ARMPIVOT_KI = 0; public static final double ARMPIVOT_KD = 0; From 6f3980292ef43f6357708994eba45f8e285cc8d2 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:44:21 -0800 Subject: [PATCH 07/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6ca35a33..fb23c31c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,7 +160,7 @@ public static class GroundArmConstants { public static final int GROUND_INTAKE_SENSOR = 48; } - public class groundSpinnyConstants { + public static class GroundSpinnyConstants { public static final double GROUND_INTAKE_SPEED = -8; public static final double GROUND_INTAKE_JITTER_SPEED = 1; public static final double FUNNEL_INTAKE_SPEED = -2; From eca0e65a980b9b9aeb15eeaa2cd9bb04376f314a Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:44:39 -0800 Subject: [PATCH 08/11] Update Constants.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fb23c31c..c6684a54 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -183,7 +183,7 @@ public static class SpinnyClawConstants { public static final int SPINNY_CLAW_MOTOR_ID = 40; } - public class visionConstants_Misc { + public static class VisionMiscConstants { // differences from center robot camera poses private static final double CAMERA_X_POS_METERS_LEFT = 0.26; private static final double CAMERA_X_POS_METERS_RIGHT = 0.27; From d05d739484741e05ca83d3aa85c91ef991aac152 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:44:59 -0800 Subject: [PATCH 09/11] Update Controls.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b3747555..e66992f8 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -594,7 +594,7 @@ private void configureElevatorBindings() { .schedule(); } - // operatorController.rightBumper().whileTrue(s.elevatorConstants.holdCoastMode()); + // operatorController.rightBumper().whileTrue(s.elevatorSubsystem.holdCoastMode()); var elevatorCoastButton = Shuffleboard.getTab("Controls") .add("Elevator Coast Mode", false) From 1bf90e8d0dc8c4e26307378d98eea2215ff91ad4 Mon Sep 17 00:00:00 2001 From: koolpoolo Date: Fri, 5 Dec 2025 13:45:49 -0800 Subject: [PATCH 10/11] Update Controls.java Co-authored-by: gemini-code-assist[bot] <176961590+gemini-code-assist[bot]@users.noreply.github.com> --- src/main/java/frc/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index e66992f8..18bf6e20 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -606,7 +606,7 @@ private void configureElevatorBindings() { // new Trigger(() -> elevatorZeroButton.get()) // .debounce(1, DebounceType.kRising) // .and(RobotModeTriggers.disabled()) - // .onTrue(s.elevatorConstants.resetPosZero()); + // .onTrue(s.elevatorSubsystem.resetPosZero()); } private void configureArmPivotBindings() { From 1b18ef7c3748f812785f7f1e3e38d8a22d9c391f Mon Sep 17 00:00:00 2001 From: koolpoolo <181432562+koolpoolo@users.noreply.github.com> Date: Mon, 8 Dec 2025 16:09:25 -0800 Subject: [PATCH 11/11] made it pass gradle build --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/Controls.java | 49 +++-- src/main/java/frc/robot/Robot.java | 4 +- .../java/frc/robot/sensors/ArmSensor.java | 4 +- .../java/frc/robot/sensors/BranchSensors.java | 6 +- .../java/frc/robot/sensors/ElevatorLight.java | 4 +- .../java/frc/robot/sensors/IntakeSensor.java | 4 +- .../java/frc/robot/subsystems/ArmPivot.java | 26 +-- .../java/frc/robot/subsystems/ClimbPivot.java | 56 +++--- .../robot/subsystems/ElevatorSubsystem.java | 38 ++-- .../java/frc/robot/subsystems/GroundArm.java | 24 +-- .../frc/robot/subsystems/GroundSpinny.java | 26 +-- .../java/frc/robot/subsystems/SpinnyClaw.java | 28 +-- .../frc/robot/subsystems/SuperStructure.java | 172 +++++++++--------- .../frc/robot/subsystems/VisionSubsystem.java | 15 +- 15 files changed, 229 insertions(+), 229 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c6684a54..239b0c17 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.util.Units; public class Constants { + private Constants() {} + public static class ArmPivotConstants { // Presets public static final double ARMPIVOT_KP = 38.5; // previously 50 diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 18bf6e20..4c6be264 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -24,10 +24,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.*; -import frc.robot.Constants.armPivotConstants; -import frc.robot.Constants.elevatorConstants; -import frc.robot.Constants.groundArmConstants; +import frc.robot.Constants.ArmPivotConstants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.GroundArmConstants; import frc.robot.generated.BonkTunerConstants; import frc.robot.generated.CompTunerConstants; import frc.robot.subsystems.SuperStructure; @@ -514,7 +513,7 @@ private void configureElevatorBindings() { .whileTrue( s.elevatorSubsystem .startMovingVoltage( - () -> Volts.of(elevatorConstants.UP_VOLTAGE * -operatorController.getLeftY())) + () -> Volts.of(ElevatorConstants.UP_VOLTAGE * -operatorController.getLeftY())) .withName("Elevator Manual Control")); s.elevatorSubsystem.setRumble( (rumble) -> { @@ -525,55 +524,55 @@ private void configureElevatorBindings() { .and(elevatorTestController.y()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.CORAL_LEVEL_FOUR_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_FOUR_POS) .withName("Elevator L4")); connected(elevatorTestController) .and(elevatorTestController.x()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.CORAL_LEVEL_THREE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_THREE_POS) .withName("Elevator L3")); connected(elevatorTestController) .and(elevatorTestController.b()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.CORAL_LEVEL_TWO_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_TWO_POS) .withName("Elevator L2")); connected(elevatorTestController) .and(elevatorTestController.a()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_ONE_POS) .withName("Elevator L1")); connected(elevatorTestController) .and(elevatorTestController.rightBumper()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.CORAL_INTAKE_POS) + .setLevel(ElevatorConstants.CORAL_INTAKE_POS) .withName("Elevator IntakePos")); connected(elevatorTestController) .and(elevatorTestController.povUp()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR) + .setLevel(ElevatorConstants.ALGAE_LEVEL_THREE_FOUR) .withName("Elevator Algae L3-L4")); connected(elevatorTestController) .and(elevatorTestController.povLeft()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE) + .setLevel(ElevatorConstants.ALGAE_LEVEL_TWO_THREE) .withName("Elevator Algae L2-L3")); connected(elevatorTestController) .and(elevatorTestController.povRight()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.ALGAE_STOWED) + .setLevel(ElevatorConstants.ALGAE_STOWED) .withName("Elevator Algae Stowed")); connected(elevatorTestController) .and(elevatorTestController.povDown()) .onTrue( s.elevatorSubsystem - .setLevel(elevatorConstants.ALGAE_PROCESSOR_SCORE) + .setLevel(ElevatorConstants.ALGAE_PROCESSOR_SCORE) .withName("Elevator Processor")); connected(elevatorTestController) .and(elevatorTestController.leftBumper()) @@ -631,43 +630,43 @@ private void configureArmPivotBindings() { .and(armPivotSpinnyClawController.povRight()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L4) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L4) .withName("Arm L4 Preset")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povLeft()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L3) .withName("Arm L3 Preset")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povUp()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .withName("Arm Preset Up")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.povDown()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withName("Arm Preset Down")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.y()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE) + .moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_REMOVE) .withName("Algae Preset Remove")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.b()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE) + .moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE) .withName("Algae Preset Score")); connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.a()) .onTrue( s.armPivotSubsystem - .moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_STOWED) + .moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_STOWED) .withName("Algae Preset Stowed")); } @@ -862,14 +861,14 @@ private void configureGroundArmBindings() { } s.groundArm.setDefaultCommand( s.groundArm - .moveToPosition(groundArmConstants.STOWED_POSITION) + .moveToPosition(GroundArmConstants.STOWED_POSITION) .andThen(Commands.idle()) .withName("Ground stowed position wait")); operatorController .rightBumper() .whileTrue( s.groundArm - .moveToPosition(groundArmConstants.GROUND_POSITION) + .moveToPosition(GroundArmConstants.GROUND_POSITION) .andThen(Commands.idle()) .withName("ground up position")); } @@ -1123,7 +1122,7 @@ private void configureSoloControllerBindings() { .povUp() .whileTrue( s.groundArm - .moveToPosition(groundArmConstants.GROUND_POSITION) + .moveToPosition(GroundArmConstants.GROUND_POSITION) .andThen(Commands.idle()) .withName("ground up position")); // Arm manual @@ -1139,7 +1138,7 @@ private void configureSoloControllerBindings() { .whileTrue( s.elevatorSubsystem .startMovingVoltage( - () -> Volts.of(elevatorConstants.UP_VOLTAGE * -soloController.getLeftY())) + () -> Volts.of(ElevatorConstants.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5e9452b4..213a6c93 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.Constants.visionConstants_Misc; +import frc.robot.Constants.VisionMiscConstants; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoBuilderConfig; @@ -50,7 +50,7 @@ protected Robot() { instance = this; robotType = RobotType.getCurrent(); CanBridge.runTCP(); - PDH = new PowerDistribution(visionConstants_Misc.PDH_ID, ModuleType.kRev); + PDH = new PowerDistribution(VisionMiscConstants.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); diff --git a/src/main/java/frc/robot/sensors/ArmSensor.java b/src/main/java/frc/robot/sensors/ArmSensor.java index 345738a7..4b69565c 100644 --- a/src/main/java/frc/robot/sensors/ArmSensor.java +++ b/src/main/java/frc/robot/sensors/ArmSensor.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.armPivotConstants; +import frc.robot.Constants.ArmPivotConstants; public class ArmSensor { @@ -22,7 +22,7 @@ public class ArmSensor { private static final double CLAW_UPPER_LIMIT = 0.13; public ArmSensor() { - mainSensor = new LaserCan(armPivotConstants.MAIN_ARM_SENSOR); + mainSensor = new LaserCan(ArmPivotConstants.MAIN_ARM_SENSOR); ConfigureSensor(mainSensor); ShuffleboardTab tab = Shuffleboard.getTab("ArmSensor"); tab.addBoolean("inTrough", inTrough()); diff --git a/src/main/java/frc/robot/sensors/BranchSensors.java b/src/main/java/frc/robot/sensors/BranchSensors.java index 47d34755..55767a39 100644 --- a/src/main/java/frc/robot/sensors/BranchSensors.java +++ b/src/main/java/frc/robot/sensors/BranchSensors.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.visionConstants_Misc; +import frc.robot.Constants.VisionMiscConstants; public class BranchSensors { @@ -23,8 +23,8 @@ public class BranchSensors { private static final double LEFT_SENSOR_MAX = 0.35; public BranchSensors() { - leftSensor = new LaserCan(visionConstants_Misc.BRANCH_SENSOR_LEFT); - rightSensor = new LaserCan(visionConstants_Misc.BRANCH_SENSOR_RIGHT); + leftSensor = new LaserCan(VisionMiscConstants.BRANCH_SENSOR_LEFT); + rightSensor = new LaserCan(VisionMiscConstants.BRANCH_SENSOR_RIGHT); ConfigureSensor(leftSensor); ConfigureSensor(rightSensor); ShuffleboardTab tab = Shuffleboard.getTab("BranchSensor"); diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index 3809a678..ae510b3e 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.visionConstants_Misc; +import frc.robot.Constants.VisionMiscConstants; import frc.robot.util.SoloScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -39,7 +39,7 @@ public class ElevatorLight extends SubsystemBase { public ElevatorLight() { - candle = new CANdle(visionConstants_Misc.ELEVATOR_LED); + candle = new CANdle(VisionMiscConstants.ELEVATOR_LED); configureCandle(); candle.clearAnimation(0); candle.setLEDs(128, 128, 128); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 9759ef9a..79b91b30 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.groundArmConstants; +import frc.robot.Constants.GroundArmConstants; public class IntakeSensor { @@ -19,7 +19,7 @@ public class IntakeSensor { private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { - intakeSensor = new LaserCan(groundArmConstants.GROUND_INTAKE_SENSOR); + intakeSensor = new LaserCan(GroundArmConstants.GROUND_INTAKE_SENSOR); ConfigureSensor(intakeSensor); ShuffleboardTab tab = Shuffleboard.getTab("IntakeSensor"); tab.addBoolean("In Intake", inIntake()); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index e8cfa58e..1c5b7b14 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.Constants.armPivotConstants; +import frc.robot.Constants.ArmPivotConstants; import java.util.function.Supplier; public class ArmPivot extends SubsystemBase { @@ -48,7 +48,7 @@ public class ArmPivot extends SubsystemBase { // Arm Pivot Contructor public ArmPivot() { - motor = new TalonFX(armPivotConstants.ARM_PIVOT_MOTOR_ID); + motor = new TalonFX(ArmPivotConstants.ARM_PIVOT_MOTOR_ID); routine = new SysIdRoutine( new SysIdRoutine.Config(Volts.of(1).div(Seconds.of(1)), Volts.of(1), Seconds.of(2)), @@ -90,7 +90,7 @@ private Command setTargetPosition(double pos) { // Boolean returning whether or not the arm is at the desired position public boolean atPosition(double position) { return MathUtil.isNear( - position, getCurrentPosition(), armPivotConstants.ARMPIVOT_POS_TOLERANCE); + position, getCurrentPosition(), ArmPivotConstants.ARMPIVOT_POS_TOLERANCE); } // Returns the current target position (position the arm is to move to) @@ -119,7 +119,7 @@ public Command moveToPosition(double position) { public Trigger atAngle(double position) { return new Trigger( - () -> Math.abs(getCurrentPosition() - position) < armPivotConstants.ARMPIVOT_POS_TOLERANCE); + () -> Math.abs(getCurrentPosition() - position) < ArmPivotConstants.ARMPIVOT_POS_TOLERANCE); } // (+) is to move arm up, and (-) is down. sets a voltage to pass to motor to move @@ -149,9 +149,9 @@ public void factoryDefaults() { var talonFXConfiguration = new TalonFXConfiguration(); // specifies what the sensor is, what port its on, and what the gearing ratio for the sensor is // relative to the motor - talonFXConfiguration.Feedback.FeedbackRemoteSensorID = armPivotConstants.ARM_PIVOT_CANDI_ID; + talonFXConfiguration.Feedback.FeedbackRemoteSensorID = ArmPivotConstants.ARM_PIVOT_CANDI_ID; talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANdiPWM1; - talonFXConfiguration.Feedback.RotorToSensorRatio = 1 / armPivotConstants.ARM_RATIO; + talonFXConfiguration.Feedback.RotorToSensorRatio = 1 / ArmPivotConstants.ARM_RATIO; // Inverting motor output direction talonFXConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; @@ -166,13 +166,13 @@ public void factoryDefaults() { // PID // set slot 0 gains - talonFXConfiguration.Slot0.kS = armPivotConstants.ARMPIVOT_KS; - talonFXConfiguration.Slot0.kV = armPivotConstants.ARMPIVOT_KV; - talonFXConfiguration.Slot0.kA = armPivotConstants.ARMPIVOT_KA; - talonFXConfiguration.Slot0.kP = armPivotConstants.ARMPIVOT_KP; - talonFXConfiguration.Slot0.kI = armPivotConstants.ARMPIVOT_KI; - talonFXConfiguration.Slot0.kD = armPivotConstants.ARMPIVOT_KD; - talonFXConfiguration.Slot0.kG = armPivotConstants.ARMPIVOT_KG; + talonFXConfiguration.Slot0.kS = ArmPivotConstants.ARMPIVOT_KS; + talonFXConfiguration.Slot0.kV = ArmPivotConstants.ARMPIVOT_KV; + talonFXConfiguration.Slot0.kA = ArmPivotConstants.ARMPIVOT_KA; + talonFXConfiguration.Slot0.kP = ArmPivotConstants.ARMPIVOT_KP; + talonFXConfiguration.Slot0.kI = ArmPivotConstants.ARMPIVOT_KI; + talonFXConfiguration.Slot0.kD = ArmPivotConstants.ARMPIVOT_KD; + talonFXConfiguration.Slot0.kG = ArmPivotConstants.ARMPIVOT_KG; talonFXConfiguration.Slot0.GravityType = GravityTypeValue.Arm_Cosine; // set Motion Magic settings in rps not mechanism units diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 1b3c2884..3d4ce74e 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.climbConstants; +import frc.robot.Constants.ClimbConstants; import java.util.function.DoubleSupplier; public class ClimbPivot extends SubsystemBase { @@ -47,9 +47,9 @@ public enum TargetPositions { // setting the starting target position private TargetPositions selectedPos = TargetPositions.STOWED; - private double maxTargetPos = climbConstants.STOWED_MAX_PRESET; - private double minTargetPos = climbConstants.STOWED_MIN_PRESET; - private double holdSpeed = climbConstants.CLIMB_HOLD_STOWED; + private double maxTargetPos = ClimbConstants.STOWED_MAX_PRESET; + private double minTargetPos = ClimbConstants.STOWED_MIN_PRESET; + private double holdSpeed = ClimbConstants.CLIMB_HOLD_STOWED; // if moveComplete is true it wont move regardless of if its in range. This is to ensure that // when disabled, when re-enabled it doesnt start moving. @@ -74,17 +74,17 @@ public enum TargetPositions { // here we go public ClimbPivot() { - motorLeft = new TalonFX(climbConstants.CLIMB_PIVOT_MOTOR_LEFT_ID); + motorLeft = new TalonFX(ClimbConstants.CLIMB_PIVOT_MOTOR_LEFT_ID); // checking to instantiate the 2nd motor if two are installed. If only one is installed, the 2nd // one is set to null. if (DUAL_MOTORS) { - motorRight = new TalonFX(climbConstants.CLIMB_PIVOT_MOTOR_RIGHT_ID); + motorRight = new TalonFX(ClimbConstants.CLIMB_PIVOT_MOTOR_RIGHT_ID); } else { motorRight = null; } // the failed climb sensor - sensor = new DigitalInput(climbConstants.CLIMB_SENSOR); + sensor = new DigitalInput(ClimbConstants.CLIMB_SENSOR); // motor configuration and shuffleboard logging methods configure(); @@ -111,17 +111,17 @@ private void configure() { TalonFXConfiguration configuration = new TalonFXConfiguration(); // remote sensor values for the WCP encoder - configuration.Feedback.FeedbackRemoteSensorID = climbConstants.CLIMB_PIVOT_CANCODER_ID; + configuration.Feedback.FeedbackRemoteSensorID = ClimbConstants.CLIMB_PIVOT_CANCODER_ID; configuration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; // previously mentioned ratio calculation configuration.Feedback.RotorToSensorRatio = - (climbConstants.MAX_ROTOR_POSITION - climbConstants.MIN_ROTOR_POSITION) - / (climbConstants.MAX_ENCODER_POSITION - climbConstants.MIN_ENCODER_POSITION); + (ClimbConstants.MAX_ROTOR_POSITION - ClimbConstants.MIN_ROTOR_POSITION) + / (ClimbConstants.MAX_ENCODER_POSITION - ClimbConstants.MIN_ENCODER_POSITION); // Set and enable current limit - configuration.CurrentLimits.StatorCurrentLimit = climbConstants.CLIMB_STATOR_CURRENT_LIMIT; + configuration.CurrentLimits.StatorCurrentLimit = ClimbConstants.CLIMB_STATOR_CURRENT_LIMIT; configuration.CurrentLimits.StatorCurrentLimitEnable = true; - configuration.CurrentLimits.SupplyCurrentLimit = climbConstants.CLIMB_SUPPLY_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = ClimbConstants.CLIMB_SUPPLY_CURRENT_LIMIT; configuration.CurrentLimits.SupplyCurrentLimitEnable = true; // Enable brake mode configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -137,8 +137,8 @@ private void configure() { // having softlimits enabled on both motors isnt great configuration.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; configuration.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - configuration.SoftwareLimitSwitch.ForwardSoftLimitThreshold = climbConstants.FORWARD_SOFT_STOP; - configuration.SoftwareLimitSwitch.ReverseSoftLimitThreshold = climbConstants.REVERSE_SOFT_STOP; + configuration.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ClimbConstants.FORWARD_SOFT_STOP; + configuration.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ClimbConstants.REVERSE_SOFT_STOP; talonFXConfigurator.apply(configuration); // OpposeMasterDirection can be changed based on climb design, not yet sure if 2nd motor will be // on opposite side @@ -156,26 +156,26 @@ private void setTargetPos(TargetPositions newTargetPos) { // if stowed, it sets the pos its going to as stowed, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.STOWED; - maxTargetPos = climbConstants.STOWED_MAX_PRESET; - minTargetPos = climbConstants.STOWED_MIN_PRESET; + maxTargetPos = ClimbConstants.STOWED_MAX_PRESET; + minTargetPos = ClimbConstants.STOWED_MIN_PRESET; moveComplete = false; } case CLIMB_OUT -> { // if climb_out, it sets the pos its going to as climb_out, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMB_OUT; - maxTargetPos = climbConstants.CLIMB_OUT_MAX_PRESET; - minTargetPos = climbConstants.CLIMB_OUT_MIN_PRESET; - holdSpeed = climbConstants.CLIMB_HOLD_STOWED; + maxTargetPos = ClimbConstants.CLIMB_OUT_MAX_PRESET; + minTargetPos = ClimbConstants.CLIMB_OUT_MIN_PRESET; + holdSpeed = ClimbConstants.CLIMB_HOLD_STOWED; moveComplete = false; } case CLIMBED -> { // if climbed, it sets the pos its going to as climbed, and sets the target positions // also makes move complete false to indicate it will move selectedPos = TargetPositions.CLIMBED; - maxTargetPos = climbConstants.CLIMBED_MAX_PRESET; - minTargetPos = climbConstants.CLIMBED_MIN_PRESET; - holdSpeed = climbConstants.CLIMB_HOLD_CLIMBOUT; + maxTargetPos = ClimbConstants.CLIMBED_MAX_PRESET; + minTargetPos = ClimbConstants.CLIMBED_MIN_PRESET; + holdSpeed = ClimbConstants.CLIMB_HOLD_CLIMBOUT; moveComplete = false; } } @@ -307,14 +307,14 @@ public void periodic() { double currentPos = getClimbPosition(); // checks what state its in position wise and returns if its in that state - if (climbConstants.CLIMB_OUT_MIN_PRESET <= currentPos - && currentPos <= climbConstants.CLIMB_OUT_MAX_PRESET) { + if (ClimbConstants.CLIMB_OUT_MIN_PRESET <= currentPos + && currentPos <= ClimbConstants.CLIMB_OUT_MAX_PRESET) { isClimbOut = true; } else { isClimbOut = false; } - if (climbConstants.STOWED_MIN_PRESET <= currentPos - && currentPos <= climbConstants.STOWED_MAX_PRESET) { + if (ClimbConstants.STOWED_MIN_PRESET <= currentPos + && currentPos <= ClimbConstants.STOWED_MAX_PRESET) { isStowed = true; } else { isStowed = false; @@ -375,11 +375,11 @@ public Command advanceClimbCheck() { if (!moveComplete) { pError = minTargetPos - getClimbPosition(); if (pError > 0) { - speedToSet = climbConstants.CLIMB_OUT_SPEED * pError * climbConstants.climbOutKp; + speedToSet = ClimbConstants.CLIMB_OUT_SPEED * pError * ClimbConstants.climbOutKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } else { - speedToSet = climbConstants.CLIMB_IN_SPEED * -pError * climbConstants.climbInKp; + speedToSet = ClimbConstants.CLIMB_IN_SPEED * -pError * ClimbConstants.climbInKp; motorLeft.set(speedToSet); setSpeed = speedToSet; } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 9a43e912..d6e8b773 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.elevatorConstants; +import frc.robot.Constants.ElevatorConstants; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -64,8 +64,8 @@ public class ElevatorSubsystem extends SubsystemBase { /** Subsystem constructor. */ public ElevatorSubsystem() { // m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse); - m_motor = new TalonFX(elevatorConstants.ELEVATOR_MOTOR_ONE, "Drivebase"); - m_motor2 = new TalonFX(elevatorConstants.ELEVATOR_MOTOR_TWO, "Drivebase"); + m_motor = new TalonFX(ElevatorConstants.ELEVATOR_MOTOR_ONE, "Drivebase"); + m_motor2 = new TalonFX(ElevatorConstants.ELEVATOR_MOTOR_TWO, "Drivebase"); m_motorOneSimState = m_motor.getSimState(); m_motorTwoSimState = m_motor2.getSimState(); motorConfigs(); @@ -134,9 +134,9 @@ public void motorConfigs() { TalonFXConfiguration configuration = new TalonFXConfiguration(); // enable stator current limit - configuration.CurrentLimits.StatorCurrentLimit = elevatorConstants.CURRENT_STATOR_LIMIT; + configuration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.CURRENT_STATOR_LIMIT; configuration.CurrentLimits.StatorCurrentLimitEnable = true; - configuration.CurrentLimits.SupplyCurrentLimit = elevatorConstants.CURRENT_SUPPLY_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.CURRENT_SUPPLY_LIMIT; configuration.CurrentLimits.SupplyCurrentLimitEnable = true; // create brake mode for motors @@ -146,12 +146,12 @@ public void motorConfigs() { talonFXConfigurator2.apply(configuration); // set slot 0 gains - configuration.Slot0.kS = elevatorConstants.ELEVATOR_KS; - configuration.Slot0.kV = elevatorConstants.ELEVATOR_KV; - configuration.Slot0.kA = elevatorConstants.ELEVATOR_KA; - configuration.Slot0.kP = elevatorConstants.ELEVATOR_KP; - configuration.Slot0.kI = elevatorConstants.ELEVATOR_KI; - configuration.Slot0.kD = elevatorConstants.ELEVATOR_KD; + configuration.Slot0.kS = ElevatorConstants.ELEVATOR_KS; + configuration.Slot0.kV = ElevatorConstants.ELEVATOR_KV; + configuration.Slot0.kA = ElevatorConstants.ELEVATOR_KA; + configuration.Slot0.kP = ElevatorConstants.ELEVATOR_KP; + configuration.Slot0.kI = ElevatorConstants.ELEVATOR_KI; + configuration.Slot0.kD = ElevatorConstants.ELEVATOR_KD; // set Motion Magic settings // Bottom to full: ~40 rotations @@ -190,7 +190,7 @@ public void setRumble(DoubleConsumer rumble) { } public boolean atPosition(double position) { - return MathUtil.isNear(position, getCurrentPosition(), elevatorConstants.POS_TOLERANCE); + return MathUtil.isNear(position, getCurrentPosition(), ElevatorConstants.POS_TOLERANCE); } public boolean getHasBeenZeroed() { @@ -218,7 +218,7 @@ private void setCurrentPosition(double pos) { } public Trigger above(double position) { - return new Trigger(() -> getCurrentPosition() >= position - elevatorConstants.POS_TOLERANCE); + return new Trigger(() -> getCurrentPosition() >= position - ElevatorConstants.POS_TOLERANCE); } public Command resetPosZero() { @@ -265,18 +265,18 @@ public Command holdCoastMode() { } public Command goUp() { - return defer(() -> setLevel(getCurrentPosition() + elevatorConstants.MANUAL)); + return defer(() -> setLevel(getCurrentPosition() + ElevatorConstants.MANUAL)); } public Command goDown() { - return defer(() -> setLevel(getCurrentPosition() - elevatorConstants.MANUAL)); + return defer(() -> setLevel(getCurrentPosition() - ElevatorConstants.MANUAL)); } public Command goUpPower(DoubleSupplier scale) { return runEnd( () -> { - m_motor.setVoltage(scale.getAsDouble() * elevatorConstants.UP_VOLTAGE); - m_motor2.setVoltage(scale.getAsDouble() * -elevatorConstants.UP_VOLTAGE); + m_motor.setVoltage(scale.getAsDouble() * ElevatorConstants.UP_VOLTAGE); + m_motor2.setVoltage(scale.getAsDouble() * -ElevatorConstants.UP_VOLTAGE); }, () -> { m_motor.stopMotor(); @@ -290,8 +290,8 @@ public Command goUpPower(DoubleSupplier scale) { public Command goDownPower(DoubleSupplier scale) { return startEnd( () -> { - m_motor.setVoltage(scale.getAsDouble() * elevatorConstants.DOWN_VOLTAGE); - m_motor2.setVoltage(scale.getAsDouble() * -elevatorConstants.DOWN_VOLTAGE); + m_motor.setVoltage(scale.getAsDouble() * ElevatorConstants.DOWN_VOLTAGE); + m_motor2.setVoltage(scale.getAsDouble() * -ElevatorConstants.DOWN_VOLTAGE); }, () -> { m_motor.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 41ca8a84..6b920f34 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.groundArmConstants; +import frc.robot.Constants.GroundArmConstants; public class GroundArm extends SubsystemBase { @@ -28,7 +28,7 @@ public class GroundArm extends SubsystemBase { private double targetPos; public GroundArm() { - motor = new TalonFX(groundArmConstants.GROUND_INTAKE_ARM_MOTOR); + motor = new TalonFX(GroundArmConstants.GROUND_INTAKE_ARM_MOTOR); configMotors(); logTabs(); } @@ -39,9 +39,9 @@ public void configMotors() { var talonFXConfiguration = new TalonFXConfiguration(); talonFXConfiguration.Feedback.FeedbackRemoteSensorID = - groundArmConstants.GROUND_INTAKE_ARM_ENCODER; + GroundArmConstants.GROUND_INTAKE_ARM_ENCODER; talonFXConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - talonFXConfiguration.Feedback.RotorToSensorRatio = groundArmConstants.ARM_RATIO; + talonFXConfiguration.Feedback.RotorToSensorRatio = GroundArmConstants.ARM_RATIO; talonFXConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -54,13 +54,13 @@ public void configMotors() { // PID // set slot 0 gains - talonFXConfiguration.Slot0.kS = groundArmConstants.ARMPIVOT_KS; - talonFXConfiguration.Slot0.kV = groundArmConstants.ARMPIVOT_KV; - talonFXConfiguration.Slot0.kA = groundArmConstants.ARMPIVOT_KA; - talonFXConfiguration.Slot0.kP = groundArmConstants.ARMPIVOT_KP; - talonFXConfiguration.Slot0.kI = groundArmConstants.ARMPIVOT_KI; - talonFXConfiguration.Slot0.kD = groundArmConstants.ARMPIVOT_KD; - talonFXConfiguration.Slot0.kG = groundArmConstants.ARMPIVOT_KG; + talonFXConfiguration.Slot0.kS = GroundArmConstants.ARMPIVOT_KS; + talonFXConfiguration.Slot0.kV = GroundArmConstants.ARMPIVOT_KV; + talonFXConfiguration.Slot0.kA = GroundArmConstants.ARMPIVOT_KA; + talonFXConfiguration.Slot0.kP = GroundArmConstants.ARMPIVOT_KP; + talonFXConfiguration.Slot0.kI = GroundArmConstants.ARMPIVOT_KI; + talonFXConfiguration.Slot0.kD = GroundArmConstants.ARMPIVOT_KD; + talonFXConfiguration.Slot0.kG = GroundArmConstants.ARMPIVOT_KG; talonFXConfiguration.Slot0.GravityType = GravityTypeValue.Arm_Cosine; // set Motion Magic settings in motor rps not mechanism units @@ -112,7 +112,7 @@ public Command setVoltage(double voltage) { public Trigger atPosition(double position) { return new Trigger( - () -> Math.abs(getCurrentPosition() - position) < groundArmConstants.POS_TOLERANCE); + () -> Math.abs(getCurrentPosition() - position) < GroundArmConstants.POS_TOLERANCE); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 7fc5705c..1e64da0e 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.groundSpinnyConstants; +import frc.robot.Constants.GroundSpinnyConstants; public class GroundSpinny extends SubsystemBase { @@ -30,7 +30,7 @@ public void logTabs() { } public GroundSpinny() { - motor = new TalonFX(groundSpinnyConstants.GROUND_INTAKE_SPINNY_MOTOR); + motor = new TalonFX(GroundSpinnyConstants.GROUND_INTAKE_SPINNY_MOTOR); configMotors(); logTabs(); } @@ -68,45 +68,45 @@ private Command holdPower(double pow) { } public Command setFunnelIntakePower() { - return setPower(groundSpinnyConstants.FUNNEL_INTAKE_SPEED).withName("set funnel intake power"); + return setPower(GroundSpinnyConstants.FUNNEL_INTAKE_SPEED).withName("set funnel intake power"); } public Command holdFunnelIntakePower() { - return holdPower(groundSpinnyConstants.FUNNEL_INTAKE_SPEED) + return holdPower(GroundSpinnyConstants.FUNNEL_INTAKE_SPEED) .withName("hold funnel intake power"); } public void imperativeSetGroundIntakePower() { - motor.setVoltage(groundSpinnyConstants.GROUND_INTAKE_SPEED); - lastSetPower = groundSpinnyConstants.GROUND_INTAKE_SPEED; + motor.setVoltage(GroundSpinnyConstants.GROUND_INTAKE_SPEED); + lastSetPower = GroundSpinnyConstants.GROUND_INTAKE_SPEED; } public Command setGroundIntakePower() { - return setPower(groundSpinnyConstants.GROUND_INTAKE_SPEED).withName("set ground intake power"); + return setPower(GroundSpinnyConstants.GROUND_INTAKE_SPEED).withName("set ground intake power"); } public Command holdGroundIntakePower() { - return holdPower(groundSpinnyConstants.GROUND_INTAKE_SPEED) + return holdPower(GroundSpinnyConstants.GROUND_INTAKE_SPEED) .withName("hold ground intake power"); } public Command setQuickHandoffExtakeSpeed() { - return setPower(groundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) + return setPower(GroundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) .withName("set quick handoff extake power"); } public Command holdQuickHandoffExtakeSpeed() { - return holdPower(groundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) + return holdPower(GroundSpinnyConstants.QUICK_HANDOFF_EXTAKE_SPEED) .withName("hold quick handoff extake power"); } public Command setGroundIntakeJitterSpeed() { - return setPower(groundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) + return setPower(GroundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) .withName("set ground intake jitter power"); } public Command holdGroundIntakeJitterSpeed() { - return holdPower(groundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) + return holdPower(GroundSpinnyConstants.GROUND_INTAKE_JITTER_SPEED) .withName("hold ground intake jitter power"); } @@ -114,7 +114,7 @@ public Trigger stalling() { return new Trigger( () -> motor.getStatorCurrent().getValueAsDouble() - > groundSpinnyConstants.STATOR_CURRENT_STALL_THRESHOLD); + > GroundSpinnyConstants.STATOR_CURRENT_STALL_THRESHOLD); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e3a9dddc..92460bb7 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.spinnyClawConstants; +import frc.robot.Constants.SpinnyClawConstants; import frc.robot.sensors.ArmSensor; import frc.robot.util.ScoringMode; import java.util.function.Supplier; @@ -37,7 +37,7 @@ public class SpinnyClaw extends SubsystemBase { private double lastSetPower; public SpinnyClaw(ArmSensor armSensor) { - motor = new TalonFX(spinnyClawConstants.SPINNY_CLAW_MOTOR_ID); + motor = new TalonFX(SpinnyClawConstants.SPINNY_CLAW_MOTOR_ID); this.armSensor = armSensor; configMotors(); logTabs(); @@ -123,56 +123,56 @@ private Command holdPower(double pow) { } public Command coralIntakePower() { - return setPower(spinnyClawConstants.CORAL_INTAKE_SPEED).withName("Intake power"); + return setPower(SpinnyClawConstants.CORAL_INTAKE_SPEED).withName("Intake power"); } public Command coralExtakePower() { - return setPower(spinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Extake power"); + return setPower(SpinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Extake power"); } public Command coralRejectPower() { - return setPower(spinnyClawConstants.CORAL_REJECT_SPEED).withName("Extake power"); + return setPower(SpinnyClawConstants.CORAL_REJECT_SPEED).withName("Extake power"); } public Command coralHoldIntakePower() { - return holdPower(spinnyClawConstants.CORAL_INTAKE_SPEED).withName("Hold intake power"); + return holdPower(SpinnyClawConstants.CORAL_INTAKE_SPEED).withName("Hold intake power"); } public Command coralHoldExtakePower() { - return holdPower(spinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Hold extake power"); + return holdPower(SpinnyClawConstants.CORAL_EXTAKE_SPEED).withName("Hold extake power"); } public Command coralLevelOneHoldExtakePower() { - return holdPower(spinnyClawConstants.CORAL_L1_EXTAKE_SPEED) + return holdPower(SpinnyClawConstants.CORAL_L1_EXTAKE_SPEED) .withName("Level 1 hold extake power"); } // algae stuff public Command algaeIntakePower() { - return setPower(spinnyClawConstants.ALGAE_INTAKE_SPEED).withName("Algae intake power"); + return setPower(SpinnyClawConstants.ALGAE_INTAKE_SPEED).withName("Algae intake power"); } public Command algaeExtakePower() { // can change to net extake maybe? also bound as general extake though - return setPower(spinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae extake power"); + return setPower(SpinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae extake power"); } public Command algaeGripIntakePower() { - return setPower(spinnyClawConstants.ALGAE_GRIP_INTAKE_SPEED) + return setPower(SpinnyClawConstants.ALGAE_GRIP_INTAKE_SPEED) .withName("Algae grip intake power"); } public Command algaeHoldExtakePower() { - return holdPower(spinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae hold extake power"); + return holdPower(SpinnyClawConstants.ALGAE_EXTAKE_SPEED).withName("Algae hold extake power"); } public Command algaeExtakeProcessorPower() { - return setPower(spinnyClawConstants.ALGAE_PROCESSOR_EXTAKE_SPEED) + return setPower(SpinnyClawConstants.ALGAE_PROCESSOR_EXTAKE_SPEED) .withName("Algae processor extake power"); } public Command algaeFlingPower() { - return setPower(spinnyClawConstants.ALGAE_FLING_SPEED).withName("Algae fling power"); + return setPower(SpinnyClawConstants.ALGAE_FLING_SPEED).withName("Algae fling power"); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index 767a9833..ec243eb6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -4,9 +4,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.armPivotConstants; -import frc.robot.Constants.elevatorConstants; -import frc.robot.Constants.groundArmConstants; +import frc.robot.Constants.ArmPivotConstants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.GroundArmConstants; import frc.robot.sensors.ArmSensor; import frc.robot.sensors.BranchSensors; import frc.robot.sensors.ElevatorLight; @@ -91,26 +91,26 @@ public Command coralLevelFour(BooleanSupplier score) { Commands.parallel( Commands.print("Pre position"), elevator - .setLevel(elevatorConstants.CORAL_LEVEL_FOUR_PRE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_FOUR_PRE_POS) .deadlineFor( // keeps spinny claw engaged until coral has been scored armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(score)), spinnyClaw.stop()) .withTimeout(0.7), repeatPrescoreScoreSwing( Commands.sequence( Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_LEVEL_FOUR_PRE_POS), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_PRE_L4)) + elevator.setLevel(ElevatorConstants.CORAL_LEVEL_FOUR_PRE_POS), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_PRE_L4)) .withDeadline( Commands.waitUntil( score)), // waits until driver presses the score button or until // auto scoring happens armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), + .until(armPivot.atAngle(ArmPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), Commands.print("Pre preIntake()"), coralPreIntake() @@ -127,22 +127,22 @@ public Command coralLevelThree(BooleanSupplier score) { // same as L4 return Commands.sequence( Commands.parallel( elevator - .setLevel(elevatorConstants.CORAL_LEVEL_THREE_PRE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_THREE_PRE_POS) .deadlineFor( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(score)), spinnyClaw.stop()) .withTimeout(0.5), repeatPrescoreScoreSwing( Commands.repeatingSequence( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L3) .withDeadline(Commands.waitUntil(score)), armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), + .until(armPivot.atAngle(ArmPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) @@ -153,22 +153,22 @@ public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 return Commands.sequence( Commands.parallel( elevator - .setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_TWO_PRE_POS) .deadlineFor( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(score)), spinnyClaw.stop()) .withTimeout(0.5), repeatPrescoreScoreSwing( Commands.sequence( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L2) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L2) .withDeadline(Commands.waitUntil(score)), armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), + .until(armPivot.atAngle(ArmPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score), coralPreIntake()) .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) @@ -178,8 +178,8 @@ public Command coralLevelTwo(BooleanSupplier score) { // same as L4 and L3 public Command coralLevelOne(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L1), + elevator.setLevel(ElevatorConstants.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L1), spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(score)), @@ -199,10 +199,10 @@ public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { return Commands.sequence( Commands.parallel( elevator - .setLevel(elevatorConstants.CORAL_LEVEL_THREE_PRE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_THREE_PRE_POS) .deadlineFor( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(ipScore) .until(score)), spinnyClaw.stop()) @@ -210,12 +210,12 @@ public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { repeatPrescoreScoreSwing( Commands.repeatingSequence( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L3) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L3) .withDeadline(Commands.waitUntil(ipScore).until(score)), armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), + .until(armPivot.atAngle(ArmPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score, ipScore), coralPreIntake()) @@ -228,10 +228,10 @@ public Command coralLevelTwo( return Commands.sequence( Commands.parallel( elevator - .setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS) + .setLevel(ElevatorConstants.CORAL_LEVEL_TWO_PRE_POS) .deadlineFor( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP) .until(ipScore) .until(score)), spinnyClaw.stop()) @@ -239,12 +239,12 @@ public Command coralLevelTwo( repeatPrescoreScoreSwing( Commands.sequence( armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L2) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L2) .withDeadline(Commands.waitUntil(ipScore).until(score)), armPivot - .moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) + .moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN) .withTimeout(1.5) - .until(armPivot.atAngle(armPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), + .until(armPivot.atAngle(ArmPivotConstants.ARMPIVOT_CORAL_POST_SCORE))), score, ipScore), coralPreIntake()) @@ -255,8 +255,8 @@ public Command coralLevelTwo( public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { return Commands.sequence( Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_LEVEL_ONE_POS), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_L1), + elevator.setLevel(ElevatorConstants.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_L1), spinnyClaw.stop()) // holds coral without wearing flywheels .withTimeout(0.5) .withDeadline(Commands.waitUntil(ipScore).until(score)), @@ -279,25 +279,25 @@ public Command groundIntake(BooleanSupplier retract) { BooleanSupplier clawFull = armSensor != null ? armSensor.inClaw() : () -> false; return Commands.sequence( Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_GROUND_INTAKE_POS), + elevator.setLevel(ElevatorConstants.CORAL_GROUND_INTAKE_POS), armPivot.moveToPosition( - armPivotConstants.ARMPIVOT_CORAL_PRESET_GROUND_INTAKE), + ArmPivotConstants.ARMPIVOT_CORAL_PRESET_GROUND_INTAKE), spinnyClaw.stop(), // just as a backup in case things are silly groundSpinny.setGroundIntakePower()) .until( elevator.above( - elevatorConstants + ElevatorConstants .MIN_EMPTY_GROUND_INTAKE)), // waits until elevator is out of the way // to lift ground intake groundArm - .moveToPosition(groundArmConstants.GROUND_POSITION) - .andThen(groundArm.setVoltage(groundArmConstants.GROUND_HOLD_VOLTAGE)) + .moveToPosition(GroundArmConstants.GROUND_POSITION) + .andThen(groundArm.setVoltage(GroundArmConstants.GROUND_HOLD_VOLTAGE)) .withDeadline( Commands.waitUntil( intakeSensor .inIntake() .or(retract))), // ends ground pickup to stow coral for scoring - groundArm.moveToPosition(groundArmConstants.STOWED_POSITION), + groundArm.moveToPosition(GroundArmConstants.STOWED_POSITION), groundSpinny.setFunnelIntakePower(), coralPreIntake()) .unless(clawFull) @@ -320,26 +320,26 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Initial setup- Move elevator high enough for ground arm to be clear, start moving // arm pivot, and start spinning the ground intake Commands.parallel( - elevator.setLevel(elevatorConstants.MIN_EMPTY_GROUND_INTAKE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), + elevator.setLevel(ElevatorConstants.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough - .until(elevator.above(elevatorConstants.MIN_EMPTY_GROUND_INTAKE)), + .until(elevator.above(ElevatorConstants.MIN_EMPTY_GROUND_INTAKE)), // Core intake sequence Commands.sequence( // Deploy the ground arm (and wait until it reaches the position). - groundArm.moveToPosition(groundArmConstants.GROUND_POSITION), + groundArm.moveToPosition(GroundArmConstants.GROUND_POSITION), // After it's deployed, apply a constant voltage to press it into the bumper // and continue. - groundArm.setVoltage(groundArmConstants.GROUND_HOLD_VOLTAGE), + groundArm.setVoltage(GroundArmConstants.GROUND_HOLD_VOLTAGE), // Grabbing segment Commands.parallel( // These three are the initial setup: Move elevator down to the handoff // height, make sure armPivot finishes moving to the right height, and // spin claw - elevator.setLevel(elevatorConstants.CORAL_QUICK_INTAKE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), + elevator.setLevel(ElevatorConstants.CORAL_QUICK_INTAKE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.coralIntakePower(), // Run the intake with occassional jitters. (This is stopped when the core // intake sequence is stopped.) @@ -365,16 +365,16 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Retract the groundArm all the way to stowed (so that we aren't slowing down to stop // in the middle), but move on to the next command when we're at the handoff point. groundArm - .moveToPosition(groundArmConstants.STOWED_POSITION) - .until(groundArm.atPosition(groundArmConstants.QUICK_INTAKE_POSITION)), + .moveToPosition(GroundArmConstants.STOWED_POSITION) + .until(groundArm.atPosition(GroundArmConstants.QUICK_INTAKE_POSITION)), // Spin groundSpinny out, but skip if we lost the coral. groundSpinny.setQuickHandoffExtakeSpeed().onlyIf(armSensor.inClaw()), // Go back to stow, but skip if we lost the coral. coralStow().onlyIf(armSensor.inClaw()), // If we lost the coral, reset for a quick retry. Commands.parallel( - elevator.setLevel(elevatorConstants.MIN_EMPTY_GROUND_INTAKE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), + elevator.setLevel(ElevatorConstants.MIN_EMPTY_GROUND_INTAKE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_QUICK_INTAKE), spinnyClaw.stop()) .onlyIf(armSensor.inClaw().negate())) // Don't run the entire sequence at all if the claw is full. @@ -389,23 +389,23 @@ public Command coralStow() { BranchHeight currentBranchHeight = branchHeight.get(); double elevatorHeight = switch (currentBranchHeight) { // used to stow closer to the scoring height - case CORAL_LEVEL_FOUR -> elevatorConstants.CORAL_LEVEL_THREE_PRE_POS; - case CORAL_LEVEL_THREE -> elevatorConstants.CORAL_LEVEL_THREE_PRE_POS; - case CORAL_LEVEL_TWO -> elevatorConstants.CORAL_LEVEL_TWO_PRE_POS; - case CORAL_LEVEL_ONE -> elevatorConstants.CORAL_LEVEL_ONE_POS; + case CORAL_LEVEL_FOUR -> ElevatorConstants.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_THREE -> ElevatorConstants.CORAL_LEVEL_THREE_PRE_POS; + case CORAL_LEVEL_TWO -> ElevatorConstants.CORAL_LEVEL_TWO_PRE_POS; + case CORAL_LEVEL_ONE -> ElevatorConstants.CORAL_LEVEL_ONE_POS; }; - if (elevatorHeight > elevatorConstants.CORAL_PRE_INTAKE) { + if (elevatorHeight > ElevatorConstants.CORAL_PRE_INTAKE) { return Commands.parallel( elevator.setLevel(elevatorHeight), Commands.sequence( // waits to raise the arm until elevator is above preintake - Commands.waitUntil(elevator.above(elevatorConstants.CORAL_PRE_INTAKE)), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), + Commands.waitUntil(elevator.above(ElevatorConstants.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), spinnyClaw.stop()); } else { return Commands.parallel( Commands.sequence( - elevator.setLevel(elevatorConstants.CORAL_PRE_INTAKE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP), + elevator.setLevel(ElevatorConstants.CORAL_PRE_INTAKE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP), elevator.setLevel(elevatorHeight)), spinnyClaw.stop()); } @@ -418,10 +418,10 @@ public Command autoCoralStow() { return Commands.defer( () -> Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_LEVEL_TWO_PRE_POS), + elevator.setLevel(ElevatorConstants.CORAL_LEVEL_TWO_PRE_POS), Commands.sequence( - Commands.waitUntil(elevator.above(elevatorConstants.CORAL_PRE_INTAKE)), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), + Commands.waitUntil(elevator.above(ElevatorConstants.CORAL_PRE_INTAKE)), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_UP)), spinnyClaw.stop()), Set.of(elevator, armPivot, spinnyClaw)) .withName("Coral Stow"); @@ -429,8 +429,8 @@ public Command autoCoralStow() { public Command coralPreIntake() { return Commands.parallel( - elevator.setLevel(elevatorConstants.CORAL_PRE_INTAKE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN), + elevator.setLevel(ElevatorConstants.CORAL_PRE_INTAKE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN), spinnyClaw.coralRejectPower()) // keeps coral out while waiting to intake .andThen(Commands.print("end of preIntake()")) .withName("PreIntake"); @@ -441,8 +441,8 @@ public Command coralPreIntake() { // in preintake position return new Trigger( () -> - elevator.atPosition(elevatorConstants.CORAL_PRE_INTAKE) - && armPivot.atPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)); + elevator.atPosition(ElevatorConstants.CORAL_PRE_INTAKE) + && armPivot.atPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)); } public Command coralIntake() { // yummy coral @@ -450,8 +450,8 @@ public Command coralIntake() { // yummy coral Commands.sequence( Commands.parallel( spinnyClaw.coralIntakePower(), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), - elevator.setLevel(elevatorConstants.CORAL_INTAKE_POS)), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), + elevator.setLevel(ElevatorConstants.CORAL_INTAKE_POS)), coralStow()) .withName("Coral Intake"); } @@ -461,8 +461,8 @@ public Command autoCoralIntake() { // yummy coral Commands.sequence( Commands.parallel( spinnyClaw.coralIntakePower(), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), - elevator.setLevel(elevatorConstants.CORAL_INTAKE_POS)), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_CORAL_PRESET_DOWN)), + elevator.setLevel(ElevatorConstants.CORAL_INTAKE_POS)), autoCoralStow()) .withName("Coral Intake"); } @@ -471,8 +471,8 @@ public Command algaeLevelThreeFourIntake() { // removes algae from upper reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE), - elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR))) + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_REMOVE), + elevator.setLevel(ElevatorConstants.ALGAE_LEVEL_THREE_FOUR))) .withName("Algae L3-L4 Intake"); } @@ -480,8 +480,8 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.sequence( Commands.parallel( spinnyClaw.algaeIntakePower(), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_REMOVE), - elevator.setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE))) + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_REMOVE), + elevator.setLevel(ElevatorConstants.ALGAE_LEVEL_TWO_THREE))) .withName("Algae L2-L3 Intake"); } @@ -490,15 +490,15 @@ public Command algaeLevelTwoThreeIntake() { // removes algae from lower reef return Commands.parallel( spinnyClaw.algaeIntakePower(), Commands.sequence( - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_GROUND_INTAKE), - elevator.setLevel(elevatorConstants.ALGAE_GROUND_INTAKE))) + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_GROUND_INTAKE), + elevator.setLevel(ElevatorConstants.ALGAE_GROUND_INTAKE))) .withName("Algae Ground Intake"); } public Command algaeStow() { // holds algae return Commands.parallel( - elevator.setLevel(elevatorConstants.ALGAE_STOWED), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_STOWED), + elevator.setLevel(ElevatorConstants.ALGAE_STOWED), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_STOWED), spinnyClaw.algaeGripIntakePower()) .deadlineFor(colorSet(255, 255, 255, "White - Stowed").asProxy()) .withName("Algae Stow"); @@ -509,8 +509,8 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p Commands.parallel( spinnyClaw.algaeGripIntakePower(), Commands.sequence( - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE), - elevator.setLevel(elevatorConstants.ALGAE_PROCESSOR_SCORE))), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_PROCESSOR_SCORE), + elevator.setLevel(ElevatorConstants.ALGAE_PROCESSOR_SCORE))), Commands.waitUntil(score), spinnyClaw.algaeExtakeProcessorPower()) .withName("Algae Processor Score"); @@ -519,17 +519,17 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p public Command algaeNetScore(BooleanSupplier score) { return Commands.sequence( Commands.parallel( - elevator.setLevel(elevatorConstants.ALGAE_NET_SCORE), - armPivot.moveToPosition(armPivotConstants.ARMPIVOT_ALGAE_NET_SCORE), + elevator.setLevel(ElevatorConstants.ALGAE_NET_SCORE), + armPivot.moveToPosition(ArmPivotConstants.ARMPIVOT_ALGAE_NET_SCORE), spinnyClaw.algaeIntakePower()), Commands.waitUntil(score), spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), Commands.waitSeconds(0.7), armPivot.moveToPosition( - armPivotConstants + ArmPivotConstants .ARMPIVOT_CORAL_PRESET_UP), // added to prevent hitting the barge after scoring // net - elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR)) + elevator.setLevel(ElevatorConstants.ALGAE_LEVEL_THREE_FOUR)) .withName("Algae Net Score"); } @@ -538,7 +538,7 @@ public Command algaeNetScore(BooleanSupplier score) { Commands.parallel( spinnyClaw.algaeFlingPower(), armPivot.moveToPosition(ArmPivot.ALGAE_FLING), - elevator.setLevel(elevatorConstants.ALGAE_LEVEL_THREE_FOUR_FLING)), + elevator.setLevel(ElevatorConstants.ALGAE_LEVEL_THREE_FOUR_FLING)), Commands.waitUntil(finish), algaeStow()) .withName("Algae L3-L4 Fling"); @@ -549,7 +549,7 @@ public Command algaeLevelTwoThreeFling(BooleanSupplier finish) { Commands.parallel( spinnyClaw.algaeFlingPower(), armPivot.moveToPosition(ArmPivot.ALGAE_FLING), - elevator.setLevel(elevatorConstants.ALGAE_LEVEL_TWO_THREE_FLING)), + elevator.setLevel(ElevatorConstants.ALGAE_LEVEL_TWO_THREE_FLING)), Commands.waitUntil(finish), algaeStow()) .withName("Algae L2-L3 Fling"); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 3ef06e3f..8801535c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.visionConstants_Misc; +import frc.robot.Constants.VisionMiscConstants; import java.util.Optional; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -84,8 +84,8 @@ public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) { this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = robotField.getObject("RawVision"); // cameras init hardware wise - leftCamera = new PhotonCamera(visionConstants_Misc.LEFT_CAM); - rightCamera = new PhotonCamera(visionConstants_Misc.RIGHT_CAM); + leftCamera = new PhotonCamera(VisionMiscConstants.LEFT_CAM); + rightCamera = new PhotonCamera(VisionMiscConstants.RIGHT_CAM); // pose estimator inits for cameras with full field, multi-tag april tag detection and camera // differences from center robot // pose estimator is used to estimate the robot's position on the field based on the cameras @@ -93,12 +93,12 @@ public VisionSubsystem(DrivebaseWrapper aprilTagsHelper) { new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - visionConstants_Misc.ROBOT_TO_CAM_LEFT); + VisionMiscConstants.ROBOT_TO_CAM_LEFT); photonPoseEstimatorRightCamera = new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - visionConstants_Misc.ROBOT_TO_CAM_RIGHT); + VisionMiscConstants.ROBOT_TO_CAM_RIGHT); // vision shuffle board tab creation ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags"); @@ -196,10 +196,9 @@ private void process( TimestampSeconds, // start with STANDARD_DEVS, and for every meter of distance past 1 meter, add another // DISTANCE_SC_STANDARD_DEVS to the standard devs - visionConstants_Misc - .DISTANCE_SC_STANDARD_DEVS + VisionMiscConstants.DISTANCE_SC_STANDARD_DEVS .times(Math.max(0, Distance - 1)) - .plus(visionConstants_Misc.STANDARD_DEVS)); + .plus(VisionMiscConstants.STANDARD_DEVS)); // sets estimated current pose to estimated vision pose robotField.setRobotPose(aprilTagsHelper.getEstimatedPosition()); // updates shuffleboard values