Intake uses PRM, Shoot Data updated, LL Logic for botpose updates

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

@@ -82,8 +82,8 @@ public class RobotContainer {
   public final ShuffleboardTab SHOOTER_TAB = Shuffleboard.getTab("SHOOTER");
   public final ShuffleboardTab PROTO_TAB = Shuffleboard.getTab("PROTO");
   public final String SPEAKER_LIMELIGHT = "limelight-speaker";
+  public final String RIGHT_LIMELIGHT = "limelight-right";
   public final String AMP_LIMELIGHT = "limelight-amp";
-  public final String RIGHT_LIMEKIGHT = "limelight-right";
   public final Vision INTAKE_PHOTON = new Vision("Arducam_OV9782_USB_Camera", 0.651830, 60);
   public final CommandSwerveDrivetrain DRIVETRAIN = DriveConstants.DriveTrain; // My drivetrain
   public final Candles CANDLES = new Candles(Constants.LEFT_CANDLE, Constants.RIGHT_CANDLE);
@@ -332,7 +332,7 @@ public void configureShuffleboard() {
                       SHOOTER.stopFeeder();
                       SHOOTER.stopShooter();
                     })));
-    PHOTON_TAB.addDouble("DISTANCE_TO_SPEAKER", () -> Util.getDistance(SPEAKER_LIMELIGHT));
+    PHOTON_TAB.addDouble("DISTANCE_TO_SPEAKER", () -> Util.getDistanceToSpeaker());
 
     COMMANDS_TAB.add(
         "Coast Swerve",
@@ -510,6 +510,7 @@ public void configureNamedCommands() {
         new IntakeNoteSequenceAuto(
             SHOOTER, INTAKE, WRIST,
             ELEVATOR)); // new InstantCommand(() -> aimAtTargetAuto = true)).andThen()
+    NamedCommands.registerCommand("c", new ShootSubwoofer(ELEVATOR, WRIST, SHOOTER).asProxy());
   }
 
   public void addAuto(String autoName) {
@@ -533,6 +534,7 @@ public void updatePoseVision(final String limelight) {
     if (Math.abs(currentSpeed.vxMetersPerSecond) > 2.0
         || Math.abs(currentSpeed.vyMetersPerSecond) > 2.0
         || Math.abs(currentSpeed.omegaRadiansPerSecond) > Math.PI) {
+          DriverStation.reportWarning("Ignoring Pose update due to speed", false);
       return;
     }
     Limelight.PoseEstimate botPose = Limelight.getBotPoseEstimate_wpiBlue(limelight);
@@ -549,13 +551,15 @@ public void updatePoseVision(final String limelight) {
     // Reject pose from long disance or high ambiguity.
     if ((botPose.tagCount == 1
             && (botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept
-                || botPose.rawFiducials[0].ambiguity >= 0.9))
+                /*|| botPose.rawFiducials[0].ambiguity >= 0.9*/))
         || (botPose.tagCount >= 2
             && botPose.avgTagDist > Constants.Vision.maxMutiTagDistToAccept)) {
       return;
     }
     // Trust close multi tag pose when disabled with increased confidence.
-    if (DriverStation.isDisabled()
+    if (Math.abs(currentSpeed.vxMetersPerSecond) < 0.1
+        && Math.abs(currentSpeed.vyMetersPerSecond) < 0.1
+        && Math.abs(Math.toDegrees(currentSpeed.omegaRadiansPerSecond)) < 10
         && botPose.tagCount >= 2
         && botPose.avgTagDist < Constants.Vision.maxTagDistToTrust) {
       DRIVETRAIN.addVisionMeasurement(
@@ -566,6 +570,7 @@ public void updatePoseVision(final String limelight) {
         botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation());
     // Reject a pose that is far away from the current robot pose.
     if (botPoseToPoseDistance > 0.5) {
+      System.out.println("Rejecting, Bot pose and limelight pose to far");
       return;
     }
     double tagDistanceFeet = Units.metersToFeet(botPose.avgTagDist);
@@ -574,6 +579,7 @@ public void updatePoseVision(final String limelight) {
       tagDistanceFeet *= 2;
     }
     double confidence = 0.7 + (tagDistanceFeet / 100);
+    DriverStation.reportWarning("New pose from vision: " + botPose.pose.toString(), false);
     DRIVETRAIN.addVisionMeasurement(
         botPose.pose, botPose.timestampSeconds, VecBuilder.fill(confidence, confidence, 99));
   }

src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.Wrist;
 import frc.robot.util.Util;
 
@@ -26,11 +27,10 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double degrees = Util.getInterpolatedWristAngle();
+    double degrees = Util.getInterpolatedWristAngle(Constants.Vision.SPEAKER_LIMELIGHT);
     // TODO find actual values, prevent wrist collision when the elevator is all the way down.
     DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false);
-    if (degrees > 10.0 && degrees < 35.0) {
-
+    if (degrees > 10.0 && degrees < 35.5) {
       return;
     }
     wrist.setDegrees(degrees);

src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java

@@ -38,7 +38,7 @@ public void execute() {
     // }
     // if (shooter.isCenterBroken() &&
     // validShotDebouncer.calculate(Util.isValidShot(SPEAKER_LIMELIGHT))) {
-    shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_RPM_CLOSE);
+    shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM);
     // } else {
     //   shooter.stopShooter();
     // }

src/main/java/frc/robot/commands/control/note/IntakeNoteSequence.java

@@ -28,7 +28,7 @@ public IntakeNoteSequence(Shooter shooter, Intake intake, Wrist wrist, Elevator
         new InstantCommand(
             () -> {
               shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS);
-              intake.setVolts(Constants.INTAKE_COLLECT_VOLTS_MANUAL);
+              intake.setRPM(Constants.INTAKE_RPM);
               wrist.setDegrees(21); // testing
               elevator.goHome();
             },
@@ -54,7 +54,7 @@ public IntakeNoteSequence(
         new InstantCommand(
             () -> {
               shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS_AGRESSIVE);
-              intake.setVolts(IntakeVoltage);
+              intake.setRPM(Constants.INTAKE_RPM);
               elevator.goHome();
             },
             shooter,

src/main/java/frc/robot/commands/control/note/IntakeNoteSequenceAuto.java

@@ -27,7 +27,7 @@ public IntakeNoteSequenceAuto(Shooter shooter, Intake intake, Wrist wrist, Eleva
         new InstantCommand(
             () -> {
               shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS);
-              intake.setVolts(Constants.INTAKE_COLLECT_VOLTS_MANUAL);
+              intake.setRPM(Constants.INTAKE_RPM);
               wrist.setDegrees(21); // testing
               elevator.goHome();
             },
@@ -50,7 +50,7 @@ public IntakeNoteSequenceAuto(
         new InstantCommand(
             () -> {
               shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS_AGRESSIVE);
-              intake.setVolts(Constants.INTAKE_COLLECT_VOLTS_MANUAL);
+              intake.setRPM(Constants.INTAKE_RPM);
               wrist.setDegrees(21); // testing
               elevator.goHome();
             },

src/main/java/frc/robot/commands/movement/PointAtAprilTag.java

@@ -43,8 +43,9 @@ public PointAtAprilTag( // USE FAST POINT INSTEAD. DO NOT USE COMMAND IT IS UNRE
     this.velocityXSupplier = velocityXSupplier;
     this.velocityYSupplier = velocityYSupplier;
     this.rotationSupplier = rotationSupplier;
-    THETA_CONTROLLER = new PIDController(0.183, 0.1, 0.0013);
+    THETA_CONTROLLER = new PIDController(0.33, 0.0, 0.0); // (0.183, 0.1, 0.0013)
     THETA_CONTROLLER.enableContinuousInput(-180, 180);
+    THETA_CONTROLLER.setTolerance(0.01, 0.01);
     addRequirements(drivetrain);
   }
 

src/main/java/frc/robot/commands/movement/SwerveCommand.java

@@ -76,16 +76,16 @@ public void execute() {
       isCardinalLocking = true;
       switch (POV_DEGREE.getAsInt()) {
         case 0:
-          setPoint = 0;
+          setPoint = 180;
           break;
         case 90:
-          setPoint = -90;
+          setPoint = 90;
           break;
         case 180:
-          setPoint = 180;
+          setPoint = 0;
           break;
         case 270:
-          setPoint = 90;
+          setPoint = -90;
           break;
         default:
           break;

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

@@ -53,7 +53,8 @@ public class Constants {
   public static final int WRIST_ID = 54;
 
   public static final double INTAKE_COLLECT_VOLTS = -6; // 6
-  public static final double INTAKE_COLLECT_VOLTS_MANUAL = -7; // 6
+  public static final double INTAKE_COLLECT_VOLTS_MANUAL = -8; // 6 //-9
+  public static final double INTAKE_RPM = -4500; // 6 //-9
 
   public static class Trap {
     public static final double TRAP_DEBOUNCE_TIME = 0.06;
@@ -65,7 +66,7 @@ public static class Trap {
   }
 
   public static class Wrist {
-    public static final double WRIST_KP = 250.0;
+    public static final double WRIST_KP = 200.0;
     public static final double WRIST_KI = 0.0;
     public static final double WRIST_KD = 0.01;
     public static final double WRIST_KV = 0.03;
@@ -95,7 +96,7 @@ public static class Wrist {
   public static class Shooter {
     public static final double FEEDER_FEEDFWD_VOLTS = 4; // 6 // 4
     public static final double FEEDER_FEEDFWD_VOLTS_AGRESSIVE = 6; // 6 // 4
-    public static final double FEEDER_SHOOT_VOLTS = 5; // 4
+    public static final double FEEDER_SHOOT_VOLTS = 8; // 4
     public static final double FEEDER_RETRACT_VOLTS = -2;
 
     public static final double SHOOTER_RPM = 4500;
@@ -270,24 +271,43 @@ public static class Climber {
         new InterpolatingDouble(-20.97), new InterpolatingDouble(4.415));
 
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(2.334), new InterpolatingDouble(36.05));
+        new InterpolatingDouble(2.334), new InterpolatingDouble(35.50));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(2.571), new InterpolatingDouble(35.66));
+        new InterpolatingDouble(2.40), new InterpolatingDouble(35.10));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(2.798), new InterpolatingDouble(34.48));
+        new InterpolatingDouble(2.50), new InterpolatingDouble(34.23));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(2.982), new InterpolatingDouble(33.96));
+        new InterpolatingDouble(2.69), new InterpolatingDouble(32.87));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(3.087), new InterpolatingDouble(32.13));
+        new InterpolatingDouble(2.87), new InterpolatingDouble(31.69));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(3.203), new InterpolatingDouble(31.08));
+        new InterpolatingDouble(3.03), new InterpolatingDouble(30.24));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(3.383), new InterpolatingDouble(30.62));
+        new InterpolatingDouble(3.207), new InterpolatingDouble(28.86));
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(3.695), new InterpolatingDouble(28.92));
+        new InterpolatingDouble(3.388), new InterpolatingDouble(28.21)); //Closer values may require a slower shooter RPM Speed
     DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
-        new InterpolatingDouble(4.08), new InterpolatingDouble(27.6));
-
+        new InterpolatingDouble(3.70), new InterpolatingDouble(27.08));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(4.03), new InterpolatingDouble(25.63));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(4.23), new InterpolatingDouble(25.07));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(4.46), new InterpolatingDouble(24.65));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(4.69), new InterpolatingDouble(24.35));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(4.99), new InterpolatingDouble(23.21));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(5.21), new InterpolatingDouble(22.81));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(5.40), new InterpolatingDouble(22.92));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(5.69), new InterpolatingDouble(21.63));
+    DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(5.89), new InterpolatingDouble(21.47));
+        DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put(
+        new InterpolatingDouble(6.15), new InterpolatingDouble(21.45));
     // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put(new InterpolatingDouble(0.9), new
 
     // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put(
@@ -308,9 +328,9 @@ public static class Vision {
     public static final String SPEAKER_LIMELIGHT = "limelight-speaker";
     public static final RectanglePoseArea fieldBoundary =
         new RectanglePoseArea(new Translation2d(0, 0), new Translation2d(16.541, 8.211));
-    public static final double maxMutiTagDistToAccept = Units.feetToMeters(15.0);
-    public static final double maxTagDistToTrust = Units.feetToMeters(10.0);
+    public static final double maxMutiTagDistToAccept = Units.feetToMeters(25.0);   // 15.0
+    public static final double maxTagDistToTrust = Units.feetToMeters(15.0);    // 15.0
     public static final double maxSingleTagDistanceToAccept = Units.feetToMeters(10.0);
-    public static final Vector<N3> absoluteTrustVector = VecBuilder.fill(.2, .2, 99);
+    public static final Vector<N3> absoluteTrustVector = VecBuilder.fill(.2, .2, 1);
   }
 }

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

@@ -21,14 +21,14 @@ public Intake(final int INTAKE_OUT_ID, final int INTAKE_IN_ID) {
     INTAKE_BOTTOM = new TalonFX(INTAKE_IN_ID, "canfd");
 
     final Slot0Configs TOP_SLOT0_CFG = new Slot0Configs();
-    TOP_SLOT0_CFG.kP = 0.1;
-    TOP_SLOT0_CFG.kI = 0;
+    TOP_SLOT0_CFG.kP = 0.4;
+    TOP_SLOT0_CFG.kI = 0.6;
     TOP_SLOT0_CFG.kD = 0;
     TOP_SLOT0_CFG.kV = 0.01;
 
     final Slot0Configs BOTTOM_SLOT0_CFG = new Slot0Configs();
-    BOTTOM_SLOT0_CFG.kP = 0.1;
-    BOTTOM_SLOT0_CFG.kI = 0;
+    BOTTOM_SLOT0_CFG.kP = 0.4;
+    BOTTOM_SLOT0_CFG.kI = 0.6;
     BOTTOM_SLOT0_CFG.kD = 0;
     BOTTOM_SLOT0_CFG.kV = 0.01;
 
@@ -39,26 +39,25 @@ public Intake(final int INTAKE_OUT_ID, final int INTAKE_IN_ID) {
     TOP_CFG.CurrentLimits.StatorCurrentLimitEnable = true;
 
     final TalonFXConfiguration BOTTOM_CFG = new TalonFXConfiguration();
-    TOP_CFG.Slot0 = BOTTOM_SLOT0_CFG;
-    TOP_CFG.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.6;
-    TOP_CFG.CurrentLimits.StatorCurrentLimit = 30;
-    TOP_CFG.CurrentLimits.StatorCurrentLimitEnable = true;
+    BOTTOM_CFG.Slot0 = BOTTOM_SLOT0_CFG;
+    BOTTOM_CFG.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.6;
+    BOTTOM_CFG.CurrentLimits.StatorCurrentLimit = 30;
+    BOTTOM_CFG.CurrentLimits.StatorCurrentLimitEnable = true;
 
     INTAKE_TOP.getConfigurator().apply(TOP_CFG);
     INTAKE_BOTTOM.getConfigurator().apply(BOTTOM_CFG);
 
-    INTAKE_TOP.setInverted(false);
+    INTAKE_BOTTOM.setInverted(false);
     INTAKE_TOP.setNeutralMode(NeutralModeValue.Coast);
     INTAKE_BOTTOM.setNeutralMode(NeutralModeValue.Coast);
     // setupShuffleboard();
   }
 
   public void setRPM(double RPM) {
     // CANNOT FOLLOW. DIFFERENT PID CTRL FOR EACH ROLLER.
-    VelocityVoltage bottomCtl = new VelocityVoltage(0);
-    INTAKE_BOTTOM.setControl(bottomCtl.withVelocity(((RPM / 100) * 2048) / 600));
-    VelocityVoltage topCtl = new VelocityVoltage(0);
-    INTAKE_TOP.setControl(topCtl.withVelocity(((RPM / 100) * 2048) / 600));
+    VelocityVoltage ctl = new VelocityVoltage(0);
+    INTAKE_BOTTOM.setControl(ctl.withVelocity(RPM / 60.0));
+    INTAKE_TOP.setControl(ctl.withVelocity(RPM / 60.0));
   }
 
   public void setVolts(double VOLTS) {

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

@@ -4,6 +4,8 @@
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -125,5 +127,8 @@ public void setupShuffleboard() {
     WRIST_TAB.add("Re-Home", new ZeroWrist(this));
     WRIST_TAB.addDouble("Degrees", this::getDegrees);
     WRIST_TAB.addDouble("Error", this::getError);
+    GenericEntry entry2 = WRIST_TAB.add("Desired DEG", 26.23).getEntry();
+    WRIST_TAB.add(
+        "GoTo Desired DEG", new InstantCommand(() -> setDegrees(entry2.get().getDouble())));
   }
 }

src/main/java/frc/robot/util/Util.java

@@ -125,9 +125,9 @@ public static boolean canSeeTarget(String limelight) {
   public static void setupUtil() {}
 
   public static double getDistance(String limelight) {
-    // var result = Limelight.getBotPoseEstimate_wpiBlue(limelight);
-    // if (result.tagCount > 0) {
-    return new Pose3d(RobotContainer.get().getPose())
+    var result = Limelight.getBotPoseEstimate_wpiBlue(limelight);
+    if (result.tagCount > 0) {
+    return new Pose3d(result.pose)
         .getTranslation()
         .getDistance(
             getAllianceSpeakerCenter()
@@ -136,8 +136,8 @@ public static double getDistance(String limelight) {
                         new Translation3d(0, 0, -getAllianceSpeakerCenter().getZ()),
                         new Rotation3d()))
                 .getTranslation());
-    // }
-    // return 0.0;
+    }
+    return 0.0;
   }
 
   // TODO flex on alliance tag pose
@@ -174,7 +174,7 @@ public static double getInterpolatedWristAngle() {
 
   public static boolean isValidShot(String limelight) {
     double dist = Util.getDistance(limelight);
-    if (dist > 2.25 && dist < 5.25) {
+    if (dist > 2.0 && dist < 8.3) {
       return true;
     } else return false;
   }