From 678750314063e4f1901309f0c6ade4cf297b14d6 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 12 Feb 2024 21:37:36 -0500 Subject: [PATCH] perm(arm): Slow down movements. --- src/main/java/frc/robot/RobotContainer.java | 3 +-- src/main/java/frc/robot/arm/ArmConstants.java | 8 ++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b9a49b..6c35087 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -81,8 +81,7 @@ private void configureBindings() { .rightTrigger() .whileTrue(Commands.parallel(arm.to(ArmState.SHOOT)).andThen(shooter.shoot())); - operatorController.rightBumper().whileTrue(intake.outtake()).whileTrue(shooter.shoot()); - operatorController.rightTrigger().whileTrue(shooter.smartShoot()); + operatorController.rightBumper().whileTrue(shooter.shoot()); operatorController.a().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.AMP))); operatorController.b().onTrue(Commands.runOnce(() -> arm.setGoal(ArmState.STOW))); diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index d55a44e..198f0c0 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -24,7 +24,7 @@ public static class ShoulderMotorConstants { public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(12.5); /** Maximum angle of the shoulder joint. */ - public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(115); + public static final Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(90); /** Tolerance of the shoulder joint. */ public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); @@ -36,11 +36,11 @@ public static class ShoulderMotorConstants { public static final double KP = 36.0; /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 1.5; + public static final double MAXIMUM_SPEED = 1.0; /** Maximum acceleration of the shoulder joint in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION = - MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3); + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.5); /** Maximum speed and acceleration of the shoulder joint. */ public static final TrapezoidProfile.Constraints CONSTRAINTS = @@ -77,7 +77,7 @@ public static class ElbowMotorConstants { public static final double KP = 48.0; /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 2.0; + public static final double MAXIMUM_SPEED = 1.5; /** Maximum acceleration of the shoulder joint in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION =