Merge pull request #73 from FRC5188/R2-154-tuning-pid-constants

R2 154 tuning pid constants

Author: rhit-collinkn

src/main/java/frc/robot/Constants.java

@@ -294,6 +294,14 @@ public final class ShooterConstants {
         new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
 
     public static final double HARD_STOP_CURRENT_LIMIT = 3.0;
+
+    public static final double TOWER_ANGLE = 8.9; // degrees // Distance 120.5 Inches
+    public static final double HUB_ANGLE = 2.3; // degrees // Distance 49.5 inches
+    public static final double TRENCH_ANGLE = 9.8; // degrees
+
+    public static final double ANGLE_AT_96 = 6.8; // degrees // Distance 96 inches
+    public static final double ANGLE_AT_152 = 12.1; // degrees  // Distance 152 inches
+    public static final double ANGLE_AT_197 = 18.6; // degrees // Distance
   }
 
   public class ShooterFlywheelConstants {
@@ -315,7 +323,7 @@ public class ShooterFlywheelConstants {
 
     // Velocity PID
     private static Slot0Configs SLOT0CONFIG =
-        new Slot0Configs().withKP(3.0).withKI(0.15).withKD(0.0).withKV(0.1).withKS(8.5);
+        new Slot0Configs().withKP(3.5).withKI(0.15).withKD(0.0).withKV(0.1).withKS(8.5);
 
     public static TalonFXConfiguration getFXConfig(boolean invert) {
       TalonFXConfiguration config = new TalonFXConfiguration();
@@ -607,7 +615,7 @@ public static VisionSystemSim getSystemSim() {
   public class IntakePivotConstants {
     public static final String NAME = "Intake";
 
-    public static final Angle PICKUP_ANGLE = Degrees.of(106.0);
+    public static final Angle PICKUP_ANGLE = Degrees.of(130.0);
     public static final Angle STOW_ANGLE = Degrees.of(0.0);
 
     public static final Angle TOLERANCE = Degrees.of(1.0);

src/main/java/frc/robot/RobotContainer.java

@@ -381,7 +381,7 @@ private void configureButtonBindings() {
     // controller.x().onTrue(intake.stowAndStopRollers());
 
     // Flywheel
-    controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(20)));
+    controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(100)));
     controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0)));
 
     // Intake + Spindexer + Tower
@@ -390,7 +390,7 @@ private void configureButtonBindings() {
         .whileTrue(
             Commands.parallel(
                 // intake.runRollers(RotationsPerSecond.of(30)),
-                hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(70))));
+                hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(30))));
 
     controller
         .rightBumper()
@@ -417,7 +417,7 @@ private void configureButtonBindings() {
     // controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0)));
 
     // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude()));
-    // controller.povUp().onTrue(shooter.calibrateHood());
+    controller.povLeft().onTrue(shooter.calibrateHood());
     // controller.povLeft().onTrue(shooter.setHoodAngle(10));
     // controller.povDown().onTrue(shooter.setHoodAngle(15));
     // controller.povRight().onTrue(shooter.setHoodAngle(20));

src/main/java/frc/robot/subsystems/Shooter.java

@@ -144,6 +144,16 @@ public boolean hoodAtAngle() {
     return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE;
   }
 
+  public Command incrementHoodAngle() {
+    Angle currentHoodAngle = _hood.getPosition().plus(Degrees.of(2.5));
+    return setHoodAngle(currentHoodAngle.in(Degrees));
+  }
+
+  public Command decrementHoodAngle() {
+    Angle currentHoodAngle = _hood.getPosition().minus(Degrees.of(0.5));
+    return setHoodAngle(currentHoodAngle.in(Degrees));
+  }
+
   public boolean isAboveCurrentLimit() {
     if (Math.abs(_hood.getSupplyCurrent().in(Amps)) > ShooterConstants.HARD_STOP_CURRENT_LIMIT) {
       return true;