diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3a892d5..6c04ee9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -226,8 +226,8 @@ public class RobotContainer { operator.back().onTrue(elevator.homeCommand()); operator.start().toggleOnTrue( - climberPivot.runPivot(() -> -operator.getRightY()*0.5) - .alongWith(climberRollers.runRoller(() -> operator.getLeftY()*0.5))); + climberPivot.runPivot(() -> -operator.getRightY()) + .alongWith(climberRollers.runRoller(() -> operator.getLeftY()))); operator.a().onTrue( safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition) diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 2124be5..e7d8348 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -188,8 +188,6 @@ public class Drivetrain extends SubsystemBase { m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); - }else if(getVelocity() < 1.5){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -216,8 +214,6 @@ public class Drivetrain extends SubsystemBase { m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); - }else if(getVelocity() < 1.5){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.06, 0.06, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){