From f9dc5f3eff10c680f73140adb0737fcc66a5edef Mon Sep 17 00:00:00 2001 From: HeeistHo Date: Tue, 2 Apr 2024 19:13:42 -0400 Subject: [PATCH] [#475] - some more formatting and constant changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/command/ChasePieces.java | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3bf21c1d..55b81b9a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -356,7 +356,7 @@ public class VisionConstants { Units.feetToMeters(26.0)); public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9), Units.feetToMeters(5)); - public static final double ALIGNMENT_TOLERANCE = 8d; // TODO: make this an actual value + public static final double ALIGNMENT_TOLERANCE = 3d; // TODO: make this an actual value public static final double POINTATTAG_ALIGNMENT_TOLERANCE = 1d; public static final PIDController POINT_AIM_CONTROLLER = new PIDController(0.2, 0, 0.015, 0.01); public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0.01); diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java index 9117d438..b0a13d75 100644 --- a/src/main/java/frc/robot/command/ChasePieces.java +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -41,6 +41,7 @@ public class ChasePieces extends Command { private double collectPower; private double maxCollectPower; private double drivePower; + private double slowDrive = 0.5d; private double rotPower; private Pose2d startingPose; @@ -209,15 +210,15 @@ private void autoChase(){ if (!hasSeenTarget) { if (startingPose.getY() > VisionConstants.HALF_FIELD_HEIGHT) { if (DriverStation.getAlliance().get() == Alliance.Blue) { - drivetrain.setRobot(0.5, 0, -rotPower); + drivetrain.setRobot(slowDrive, 0, -rotPower); } else { - drivetrain.setRobot(0.5, 0, rotPower); + drivetrain.setRobot(slowDrive, 0, rotPower); } } else { if (DriverStation.getAlliance().get() == Alliance.Blue) { - drivetrain.setRobot(0.5, 0, rotPower); + drivetrain.setRobot(slowDrive, 0, rotPower); } else { - drivetrain.setRobot(0.5, 0, -rotPower); + drivetrain.setRobot(slowDrive, 0, -rotPower); } } } else { @@ -284,11 +285,11 @@ public boolean trustValues() { public boolean isFinished() { if (DriverStation.isAutonomous()) { if (DriverStation.getAlliance().get() == Alliance.Blue) { - if (drivetrain.getPose().getX() > AutonomousConstants.CHASE_BOUNDARY) { + if (drivetrain.getPose().getX() > AutonomousConstants.BLUE_CHASE_BOUNDARY) { return true; } } else { - if (drivetrain.getPose().getX() < AutonomousConstants.CHASE_BOUNDARY) { + if (drivetrain.getPose().getX() < AutonomousConstants.RED_CHASE_BOUNDARY) { return true; } }