diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cea1f61..ed21d3a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -165,6 +165,13 @@ public static class Presets { SHOOTER_DISTANCE_ANGLE.put(3.149, 35.13); SHOOTER_DISTANCE_ANGLE.put(3.500, 28.27); SHOOTER_DISTANCE_ANGLE.put(3.807, 33.56); + + //SEPARTE + + // SHOOTER_DISTANCE_ANGLE.put(1.2522372110383038, 50.09765625); + // SHOOTER_DISTANCE_ANGLE.put(2.110611636332603, 42.97694614955357); + // SHOOTER_DISTANCE_ANGLE.put(2.7223768570130944, 36.741420200892854); + // SHOOTER_DISTANCE_ANGLE.put(3.064532709736735, 35.127999441964285); }; public static final InterpolatingDoubleTreeMap ELEVATOR_DISTANCE_HEIGHT = new InterpolatingDoubleTreeMap(); @@ -177,6 +184,12 @@ public static class Presets { ELEVATOR_DISTANCE_HEIGHT.put(3.149, 0.029); ELEVATOR_DISTANCE_HEIGHT.put(3.500, 0.010); ELEVATOR_DISTANCE_HEIGHT.put(3.807, 0.009); + + //SEPARATE + // ELEVATOR_DISTANCE_HEIGHT.put(1.2522372110383038, 0.05915625); + // ELEVATOR_DISTANCE_HEIGHT.put(2.110611636332603, 0.027583007812499998); + // ELEVATOR_DISTANCE_HEIGHT.put(2.7223768570130944, 0.0274423828125); + // ELEVATOR_DISTANCE_HEIGHT.put(3.064532709736735, 0.0274541015625); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2f4adb6..4578c7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,6 +149,9 @@ private void configureBindings() { new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.OUTTAKE) ); + _operatorController.povRight().onTrue(_lerpSaver); + _operatorController.povLeft().onTrue(Commands.runOnce(_lerpSaver :: showCode)); + // operator bindings _operatorController.L1().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT)); _operatorController.L2().whileTrue(new SpinShooter(_shooterSubsystem, ShooterState.AMP)); diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index c5b8738..6039463 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -220,8 +220,6 @@ private boolean updateBotpose() { // UPDATE BOTPOSE WITH VISION if (visionBotpose.isPresent()) { - System.out.println("PRESENT"); - LimelightHelper.SetRobotOrientation( Limelights.MAIN, getHeading().getDegrees(),