diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7189454..1003408 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -262,8 +262,8 @@ public class RobotContainer { driver.a().whileTrue(indexer.shootNote(() -> 1.0)); operator.back().toggleOnTrue(shooter.manualPivot( - () -> operator.getLeftY(), - () -> operator.getRightTriggerAxis()-operator.getLeftTriggerAxis() + () -> MathUtil.clamp(-operator.getLeftY(), -0.75, 0.75), + () -> MathUtil.clamp(operator.getRightTriggerAxis()-operator.getLeftTriggerAxis(), -0.75, 0.75) )); operator.start().onTrue(shooter.zeroEncoder()); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index fea3f9b..add6a5e 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -24,7 +24,7 @@ public final class AutoConstants { new PIDConstants(kPXController), new PIDConstants(kPThetaController), kMaxSpeedMetersPerSecond, - Units.inchesToMeters(34.65), + Units.inchesToMeters(Math.sqrt(Math.pow(28-17.5, 2)+ Math.pow(28-1.75, 2))), new ReplanningConfig() ); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 5fe793b..d60bc30 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.VictorSPX; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ClimberConstants; +import frc.robot.constants.ShooterConstants; public class Climber extends SubsystemBase { private VictorSPX motor1; @@ -29,11 +30,8 @@ public class Climber extends SubsystemBase { public Command setSpeedWithSupplier(DoubleSupplier speed) { return run(() -> { - if(shooterAngle.getAsDouble() > Math.toRadians(-5.0)){ - motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); - }else{ - motor1.set(VictorSPXControlMode.PercentOutput, 0); - } + motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); + }); }