diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0adda27d..9783f9b5 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -29,7 +29,6 @@ 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; import frc.robot.subsystems.auto.BargeAlign; @@ -72,8 +71,6 @@ public class Controls { RobotType.getCurrent() == RobotType.BONK ? BonkTunerConstants.kSpeedAt12Volts.in(MetersPerSecond) : CompTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private final double MAX_ACCELERATION = 50; - private final double MAX_ROTATION_ACCELERATION = 50; // kSpeedAt12Volts desired top speed public static double MaxAngularRate = RotationsPerSecond.of(0.75) @@ -456,41 +453,41 @@ private Command getCoralBranchHeightCommand(ScoringType version) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure .coralLevelThree( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure .coralLevelTwo( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure .coralLevelOne( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); }; } else if (version == ScoringType.SOLOC_RIGHT) { return switch (branchHeight) { case CORAL_LEVEL_FOUR -> superStructure .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_THREE -> superStructure .coralLevelThree( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure .coralLevelTwo( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure .coralLevelOne( soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB)) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + .andThen(() -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE); }; } else { return switch (branchHeight) { @@ -722,7 +719,7 @@ private void configureSpinnyClawBindings() { if (s.spinnyClawSubsytem == null) { return; } - s.spinnyClawSubsytem.setScoringMode(() -> scoringMode); + // Claw controls bindings go here connected(armPivotSpinnyClawController) .and(armPivotSpinnyClawController.leftBumper()) @@ -976,7 +973,7 @@ private void configureSoloControllerBindings() { Commands.sequence( bargeScoreCommand, Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); + () -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE)); } case NO_GAME_PIECE -> { scoreCommand = @@ -994,12 +991,12 @@ private void configureSoloControllerBindings() { })); soloController .leftTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); soloController .leftTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB)); // Processor + Auto align right + Select scoring mode Coral @@ -1023,7 +1020,7 @@ private void configureSoloControllerBindings() { soloController.rightBumper()), Commands.waitSeconds(0.7), Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + () -> soloScoringMode = SoloScoringMode.NO_GAME_PIECE)) .withName("Processor score"); case NO_GAME_PIECE -> Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) @@ -1037,12 +1034,12 @@ private void configureSoloControllerBindings() { .withName("score")); soloController .rightTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); soloController .rightTrigger() - .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> soloScoringMode == SoloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB)); // Ground Intake diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..77cb3362 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.SubsystemConstants; -import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.auto.AutoBuilderConfig; import frc.robot.subsystems.auto.AutoLogic; import frc.robot.subsystems.auto.AutonomousField; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/SuperStructure.java similarity index 99% rename from src/main/java/frc/robot/subsystems/SuperStructure.java rename to src/main/java/frc/robot/SuperStructure.java index 41ec29d6..8aabfdf1 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/SuperStructure.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems; +package frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -8,6 +8,11 @@ import frc.robot.sensors.BranchSensors; import frc.robot.sensors.ElevatorLight; import frc.robot.sensors.IntakeSensor; +import frc.robot.subsystems.ArmPivot; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.GroundArm; +import frc.robot.subsystems.GroundSpinny; +import frc.robot.subsystems.SpinnyClaw; import frc.robot.util.BranchHeight; import java.util.Set; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index 260e962c..64f5e875 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -51,9 +51,6 @@ public enum TargetPositions { 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; @@ -74,7 +71,6 @@ public enum TargetPositions { private TargetPositions selectedPos = TargetPositions.STOWED; private double maxTargetPos = STOWED_MAX_PRESET; private double minTargetPos = STOWED_MIN_PRESET; - private double holdSpeed = 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. @@ -190,7 +186,6 @@ private void setTargetPos(TargetPositions newTargetPos) { selectedPos = TargetPositions.CLIMB_OUT; maxTargetPos = CLIMB_OUT_MAX_PRESET; minTargetPos = CLIMB_OUT_MIN_PRESET; - holdSpeed = CLIMB_HOLD_STOWED; moveComplete = false; } case CLIMBED -> { @@ -199,7 +194,6 @@ private void setTargetPos(TargetPositions newTargetPos) { selectedPos = TargetPositions.CLIMBED; maxTargetPos = CLIMBED_MAX_PRESET; minTargetPos = CLIMBED_MIN_PRESET; - holdSpeed = CLIMB_HOLD_CLIMBOUT; moveComplete = false; } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 371953a9..69f91caf 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -60,11 +60,8 @@ public class ElevatorSubsystem extends SubsystemBase { 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); diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..cad04ad7 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; import frc.robot.sensors.ArmSensor; -import frc.robot.util.ScoringMode; import java.util.function.Supplier; public class SpinnyClaw extends SubsystemBase { @@ -37,7 +36,6 @@ public class SpinnyClaw extends SubsystemBase { private final TalonFX motor; // ArmSensor private final ArmSensor armSensor; - private Supplier scoringMode = () -> ScoringMode.CORAL; // alerts private final Alert NotConnectedError = @@ -52,10 +50,6 @@ public SpinnyClaw(ArmSensor armSensor) { logTabs(); } - public void setScoringMode(Supplier scoringMode) { - this.scoringMode = scoringMode; - } - // (+) is to intake out, and (-) is in public Command movingVoltage(Supplier speedControl) { return run(() -> motor.setVoltage(speedControl.get().in(Volts))) diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 43e8f322..15ce47bd 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.networktables.GenericSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -25,7 +24,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; -import java.util.Optional; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -278,19 +276,4 @@ public double getTimeSinceLastReading() { public double getDistanceToTarget() { return (double) Math.round(Distance * 1000) / 1000; } - - // configured for 2025 reefscape to filter out any tag besides the reef tags depending on allaince - private static boolean BadAprilTagDetector(PhotonPipelineResult r) { - boolean isRed = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Red)); - boolean isBlue = DriverStation.getAlliance().equals(Optional.of(DriverStation.Alliance.Blue)); - for (var t : r.getTargets()) { - boolean isRedReef = 6 <= t.getFiducialId() && t.getFiducialId() <= 11; - boolean isBlueReef = 17 <= t.getFiducialId() && t.getFiducialId() <= 22; - boolean isValid = isBlueReef && !isRed || isRedReef && !isBlue; - if (!isValid) { - return true; - } - } - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java index 4592ed7d..62f25d94 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; -import frc.robot.subsystems.SuperStructure; +import frc.robot.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import frc.robot.util.AllianceUtils; import frc.robot.util.ScoringMode; diff --git a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java index 0e87d818..d03e654c 100644 --- a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java @@ -10,7 +10,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.SuperStructure; +import frc.robot.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java index d4427c06..b09245bf 100644 --- a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java @@ -41,10 +41,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = @@ -58,43 +54,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SysIdRoutine.Mechanism( output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); - /* The SysId routine to test */ private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;