End of Tuesday.

src/main/java/frc/robot/Robot.java

@@ -11,7 +11,6 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.constants.Constants;
 
 public class Robot extends TimedRobot {
   private Command autoCommand;
@@ -28,15 +27,8 @@ public void robotInit() {
 
   @Override
   public void robotPeriodic() {
+    robotContainer.updatePoseVision();
     CommandScheduler.getInstance().run();
-    robotContainer.updatePoseVision(
-      // Constants.Vision.SPEAKER_RIGHT_LIMELIGHT,
-      // Constants.Vision.AMP_LIMELIGHT,
-      Constants.Vision.SPEAKER_LIMELIGHT,
-      Constants.Vision.RIGHT_LO
-      // Constants.Vision.RIGHT_LIMELIGHT,
-      // Constants.Vision.LEFT_LIMELIGHT
-    );
   }
 
   @Override
@@ -56,7 +48,6 @@ public void autonomousInit() {
     robotContainer.CANDLES.setAnimationRight(
         new LarsonAnimation(255, 255, 255, 0, 0.1, 8, BounceMode.Front, 1));
     autoCommand = robotContainer.getAutonomousCommand();
-
     if (autoCommand != null) {
       autoCommand.schedule();
     }

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

@@ -4,8 +4,6 @@
 
 package frc.robot.commands.control.auto;
 
-import java.sql.Driver;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;

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

@@ -4,15 +4,11 @@
 
 package frc.robot.commands.control.auto;
 
-import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric;
-
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Shooter;
-import frc.robot.util.Util;
 
 public class InstantShoot extends Command {
 

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

@@ -24,23 +24,34 @@ public class PointAtAprilTag extends Command {
           .withDeadband(Constants.MaxSpeed * 0.1)
           .withRotationalDeadband(Constants.MaxAngularRate * 0.1) // Add a 10% deadband
           .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric
-  private DoubleSupplier velocityXSupplier = () -> 0.0;
+  private DoubleSupplier velocityXSupplier = () -> 0.0; // getAllianceLob
   private DoubleSupplier velocityYSupplier = () -> 0.0;
   private DoubleSupplier rotationSupplier = () -> 0.0;
   private double desiredRotation;
   private final PIDController THETA_CONTROLLER;
+  private boolean shouldLob = false;
   double rotationRate = 0;
 
-  public PointAtAprilTag( // USE FAST POINT INSTEAD. DO NOT USE COMMAND IT IS UNRELIABLE
+  public PointAtAprilTag(
       CommandSwerveDrivetrain drivetrain,
       DoubleSupplier velocityXSupplier,
       DoubleSupplier velocityYSupplier,
       DoubleSupplier rotationSupplier) {
+    this(drivetrain, velocityXSupplier, velocityYSupplier, rotationSupplier, false);
+  }
+
+  public PointAtAprilTag(
+      CommandSwerveDrivetrain drivetrain,
+      DoubleSupplier velocityXSupplier,
+      DoubleSupplier velocityYSupplier,
+      DoubleSupplier rotationSupplier,
+      boolean shouldLob) {
     this.drivetrain = drivetrain;
     this.velocityXSupplier = velocityXSupplier;
     this.velocityYSupplier = velocityYSupplier;
     this.rotationSupplier = rotationSupplier;
-    THETA_CONTROLLER = new PIDController(0.33, 0.0, 0.0); // (0.183, 0.1, 0.0013)
+    this.shouldLob = shouldLob;
+    THETA_CONTROLLER = new PIDController(0.15, 0.0, 0.0); // (0.183, 0.1, 0.0013)
     THETA_CONTROLLER.enableContinuousInput(-180, 180);
     THETA_CONTROLLER.setTolerance(0.01, 0.01);
     addRequirements(drivetrain);
@@ -57,7 +68,9 @@ public double getRotationRate() {
   public void execute() {
     Pose2d current = drivetrain.getPose();
     desiredRotation =
-        current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees();
+        shouldLob
+            ? current.getRotation().minus(Util.getRotationToAllianceLob(current)).getDegrees()
+            : current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees();
     DriverStation.reportWarning(desiredRotation + " degrees", false);
 
     // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new

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

@@ -98,6 +98,17 @@ public void execute() {
               DRIVETRAIN.getPose().getRotation().getDegrees(), THETA_CONTROLLER.getSetpoint());
     }
 
+    // WIP - trying to auto point to amp feed and then speaker depending on pose by default
+    // if (!isCardinalLocking && Util.isWithinTolerance(ROTATION_SUPPLIER.getAsDouble(), 0.0, 0.25)
+    // && RobotContainer.get().SHOOTER.isCenterBroken()) {
+    //
+    // THETA_CONTROLLER.setSetpoint(Util.getRotationToAllianceSpeaker(DRIVETRAIN.getPose()).getDegrees());
+    //   System.out.println(Util.getRotationToAllianceSpeaker(DRIVETRAIN.getPose()).getDegrees());
+    //   rotationPercentage =
+    //       THETA_CONTROLLER.calculate(
+    //           DRIVETRAIN.getPose().getRotation().getDegrees(), THETA_CONTROLLER.getSetpoint());
+    // }
+
     double rotationalVelocity = rotationPercentage;
 
     DRIVETRAIN.setControl(

src/main/java/frc/robot/commands/qol/DefaultCANdle.java

@@ -12,14 +12,13 @@
 public class DefaultCANdle extends Command {
   private final Shooter SHOOTER;
   private final Candles CANDLES;
-  private final String SPEAKER_LIMELIGHT;
   private final double BLINK_CONSTANT = 0.002; // in seconds?
 
   /** Creates a new DefaultCANdle. */
-  public DefaultCANdle(Candles CANDLES, Shooter SHOOTER, String LIMELIGHT_NAME) {
+  public DefaultCANdle(Candles CANDLES, Shooter SHOOTER) {
     this.CANDLES = CANDLES;
     this.SHOOTER = SHOOTER;
-    this.SPEAKER_LIMELIGHT = LIMELIGHT_NAME;
+
     addRequirements(CANDLES);
   }
 
@@ -40,11 +39,6 @@ public void execute() {
     }
 
     CANDLES.setColorLeft(0, 128, 128);
-    if (!Util.canSeeTarget(SPEAKER_LIMELIGHT)) {
-      CANDLES.setColorRightRed();
-      return;
-    }
-
     if (!Util.isValidShot()) {
       CANDLES.setColorRightRed();
       return;

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

@@ -8,6 +8,8 @@
 import frc.robot.util.InterpolatingDouble;
 import frc.robot.util.InterpolatingTreeMap;
 import frc.robot.util.RectanglePoseArea;
+import java.util.Arrays;
+import java.util.List;
 
 public class Constants {
 
@@ -22,9 +24,8 @@ public class Constants {
   public static final double TRAP_APRILTAG_HEIGHT = 1.315;
   public static final double AMP_APRILTAG_HEIGHT = 1.35; // 0.535
 
-  public static final double translationXSlewRate = 10.0;
-  public static final double translationYSlewRate = 10.0;
-  public static final double rotationSlewRate = 4.5;
+  public static final double translationSlewRate = 2.5;
+  //   public static final double rotationSlewRate = 10;
 
   public static final int SHOOTER_LEADER_ID = 53;
   public static final int SHOOTER_FOLLOWER_ID = 56;
@@ -62,7 +63,7 @@ public static class Trap {
     public static final double TRAP_ELEVATOR_HEIGHT_MIDWAY = 24.0;
     public static final double TRAP_WRIST_DEGREES = 118.0;
     public static final double TRAP_WRIST_DEGREES_MIDWAY = 80.0;
-    public static final double TRAP_RPM_SPEED = 450;    // 425, 475 worked but touched the top
+    public static final double TRAP_RPM_SPEED = 450; // 425, 475 worked but touched the top
   }
 
   public static class Wrist {
@@ -328,11 +329,14 @@ public static class Climber {
 
   public static class Vision {
     // public static final String SPEAKER_RIGHT_LOW_LIMELIGHT = "limelight-rightlo";
-    public static final String SPEAKER_LIMELIGHT = "limelight-speaker";
+    public static final String LEFT_LOW = "limelight-leftlo";
     public static final String RIGHT_LIMELIGHT = "limelight-right";
     public static final String LEFT_LIMELIGHT = "limelight-left";
-    public static final String AMP_LIMELIGHT = "limelight-amp";
-    public static final String RIGHT_LO = "limelight-rightlo";
+    public static final String RIGHT_LOW = "limelight-rightlo";
+    public static final List<String> LL3GS = Arrays.asList(LEFT_LOW, RIGHT_LOW);
+    public static final List<String> LL3S = Arrays.asList(RIGHT_LIMELIGHT, LEFT_LIMELIGHT);
+    public static final List<Integer> SPEAKER_TAG_IDS = Arrays.asList(3, 4, 7, 8);
+    public static final double MAX_DISTANCE_SCALING = 5.5;
     public static final RectanglePoseArea fieldBoundary =
         new RectanglePoseArea(new Translation2d(0, 0), new Translation2d(16.541, 8.211));
     public static final double maxMutiTagDistToAccept = Units.feetToMeters(25.0); // 15.0

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

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AmpSensors extends SubsystemBase {
+
+  private DigitalInput m_sensor;
+
+  /** Creates a new AmpSensors. */
+  public AmpSensors() {
+    m_sensor = new DigitalInput(9);
+    Shuffleboard.getTab("MAIN").addBoolean("Amp Sensor", m_sensor::get);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -42,9 +42,11 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
   /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
   private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
   /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
-  private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
+  private final Rotation2d RedAlliancePerspectiveRotation =
+      Rotation2d.fromDegrees(180); // 180 or 0???
   /* Keep track if we've ever applied the operator perspective before or not */
   private boolean hasAppliedOperatorPerspective = false;
+  private static Alliance alliance = Alliance.Red; // by Default be red
 
   private final SwerveRequest.ApplyChassisSpeeds AutoRequest =
       new SwerveRequest.ApplyChassisSpeeds();
@@ -206,6 +208,10 @@ private void startSimThread() {
     m_simNotifier.startPeriodic(kSimLoopPeriod);
   }
 
+  public static Alliance getAlliance() {
+    return alliance;
+  }
+
   @Override
   public void periodic() {
     /* Periodically try to apply the operator perspective */
@@ -222,6 +228,7 @@ public void periodic() {
                         ? RedAlliancePerspectiveRotation
                         : BlueAlliancePerspectiveRotation);
                 hasAppliedOperatorPerspective = true;
+                alliance = allianceColor == Alliance.Red ? Alliance.Red : Alliance.Blue;
               });
     }
   }

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

@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.util.Limelight.RawFiducial;
 import java.util.List;
 
@@ -37,6 +38,8 @@ public class Util {
   public static Pose2d TAG_14_POSE_2D; // FAR SIDE BLUE ALLIANCE TRAP
   public static Pose2d TAG_15_POSE_2D; // LEFT SIDE BLUE ALLIANCE TRAP
   public static Pose2d TAG_16_POSE_2D; // RIGHT SIDE BLUE ALLIANCE TRAP
+  public static Pose2d TAG_LOB_BLUE;
+  public static Pose2d TAG_LOB_RED;
   public static List<Pose2d> TRAP_TAGS;
   public static DriverStation.Alliance alliance;
   public static final double DEADBAND = 0.05;
@@ -98,6 +101,21 @@ public Util() {
               Util.TAG_16_POSE_2D =
                   pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d()));
             });
+    field
+        .getTagPose(5)
+        .ifPresent(
+            pose -> {
+              Util.TAG_LOB_RED =
+                  pose.toPose2d().transformBy(new Transform2d(0.0, -1.0, new Rotation2d(0.0)));
+              ;
+            });
+    field
+        .getTagPose(6)
+        .ifPresent(
+            pose -> {
+              Util.TAG_LOB_BLUE =
+                  pose.toPose2d().transformBy(new Transform2d(0.0, -1.0, new Rotation2d()));
+            });
     TRAP_TAGS =
         List.of(
             TAG_11_POSE_2D,
@@ -110,7 +128,23 @@ public Util() {
   }
 
   public static Pose3d getAllianceSpeakerCenter() {
-    return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE;
+    // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE;
+    return CommandSwerveDrivetrain.getAlliance() == DriverStation.Alliance.Blue
+        ? TAG_7_POSE
+        : TAG_4_POSE;
+  }
+
+  public static Pose2d getAllianceLob() {
+    // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE;
+    return CommandSwerveDrivetrain.getAlliance() == DriverStation.Alliance.Blue
+        ? TAG_LOB_BLUE
+        : TAG_LOB_RED;
+  }
+
+  public static Rotation2d getRotationToAllianceLob(Pose2d opose) {
+    // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE;
+    Transform2d pose = Util.getAllianceLob().minus(opose);
+    return new Rotation2d(Math.atan2(pose.getX(), pose.getY()));
   }
 
   public static boolean isWithinTolerance(
@@ -212,4 +246,14 @@ public static double maxFiducialAmbiguity(final RawFiducial[] fiducials) {
     }
     return maxAmbiguity;
   }
+
+  public static double speakerTagCount(final RawFiducial[] fiducials) {
+    int tagCount = 0;
+    for (RawFiducial fiducial : fiducials) {
+      if (Constants.Vision.SPEAKER_TAG_IDS.contains(fiducial.id)) {
+        tagCount++;
+      }
+    }
+    return tagCount;
+  }
 }