diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 226fd83..57e117c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -184,6 +184,7 @@ public static class FieldConstants { public static final double SHOOTER_SLOW_THRESHOLD = 2; public static final double TAG_DISTANCE_THRESHOLD = 3.5; + public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 2; public static final int SPEAKER_TAG_BLUE = 7; public static final int SPEAKER_TAG_RED = 4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cabe4ba..44527f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; +import frc.robot.Constants.Ports; import frc.robot.Constants.Presets; import frc.robot.commands.auto.AutoAim; import frc.robot.commands.auto.AutonShoot; @@ -53,7 +54,7 @@ public class RobotContainer { private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem); private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem(); private final IntakeSubsystem _intakeSubsystem = new IntakeSubsystem(); - private final LEDSubsystem _ledSubsystem = new LEDSubsystem(0, 100); + private final LEDSubsystem _ledSubsystem = new LEDSubsystem(Ports.LEDS, 35); private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem(); // controllers (for driver and operator) diff --git a/src/main/java/frc/robot/commands/auto/NoteAlign.java b/src/main/java/frc/robot/commands/auto/NoteAlign.java index 471ca1f..96ad24f 100644 --- a/src/main/java/frc/robot/commands/auto/NoteAlign.java +++ b/src/main/java/frc/robot/commands/auto/NoteAlign.java @@ -82,6 +82,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return _headingController.atSetpoint(); + return false; } } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 6e7f153..b4e7507 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -215,15 +215,16 @@ private void updateBotpose() { if (tagCount >= 2) { xyStds = 0.55; } - - // good distance - else if (tagDistance > 0.8 && poseDifference >= 0.5) { + + // one tag, closer distance, estimated pose is inaccurate + else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 3) { xyStds = 0.65; } - // else if (tagDistance > 0.3 && poseDifference >= 0.3) { - - // } + // one tag, closer distance, estimated pose is accurate + else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 0.5) { + xyStds = 0.85; + } else { return; @@ -288,7 +289,7 @@ public double getDriveCoeff() { public void driveChassis(ChassisSpeeds chassisSpeeds) { chassisSpeeds.vxMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vxMetersPerSecond, .01); chassisSpeeds.vyMetersPerSecond = MathUtil.applyDeadband(chassisSpeeds.vyMetersPerSecond, .01); - chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 20); + chassisSpeeds.omegaRadiansPerSecond = MathUtil.applyDeadband(chassisSpeeds.omegaRadiansPerSecond, Math.PI / 30); // IMPORTANT: X-axis and Y-axis are flipped (based on wpilib coord system) if (fieldOriented) {