Added current logging for indexer, restrict shooting when close to hub, dotproduct math to improve driver automation

simgui-ds.json

@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "enabled": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

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

@@ -69,6 +69,7 @@ public static class SimConstants {
     public static final int maxCapacity = 30;
     public static final double fuelsPerSecond = 6.7;
     public static final double loopPeriodSecs = 0.020;
+    public static final Distance closestPossibleShotDistance = Meters.of(1.5);
   }
 
   public static class SOTMConstants {

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

@@ -19,7 +19,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.Constants.FieldConstants;
@@ -43,14 +42,7 @@
 import frc.robot.util.SwerveTelemetry;
 import java.util.function.Supplier;
 
-/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
- * subsystems, commands, and trigger mappings) should be declared here.
- */
 public class RobotContainer {
-  // Replace with CommandPS4Controller or CommandJoystick if needed
   private final ExtendedCommandXboxController driverController =
       new ExtendedCommandXboxController(OperatorConstants.kDriverControllerPort);
   private final ExtendedCommandXboxController operatorController =
@@ -77,7 +69,6 @@ public class RobotContainer {
   private Pose2d goalShotTarget;
   private final Supplier<Pose2d> goalShotTargetSupplier = () -> goalShotTarget;
 
-  private boolean toggleAutomatedShooting = true;
   private boolean canPreShoot = false;
 
   @Logged(name = "Swerve")
@@ -108,7 +99,10 @@ public class RobotContainer {
   Trigger inAllianceZoneTrigger = new Trigger(() -> swerve.inAllianceZone());
   Trigger activeHubTrigger =
       new Trigger(HubTracker::isActive).or(() -> HubTracker.getMatchTime() < 0);
-  Trigger automatedShootingTrigger = new Trigger(() -> toggleAutomatedShooting);
+  Trigger automatedShootingTrigger =
+      new Trigger(() -> SmartDashboard.getBoolean("Automated Shooting Toggle", false));
+
+  Trigger tooCloseToHubTrigger = new Trigger(() -> swerve.tooCloseToHub());
 
   Trigger preShiftShoot =
       new Trigger(
@@ -124,16 +118,15 @@ public class RobotContainer {
 
                 double timeOfFlight = SOTMConstants.timeOfFlightMap.get(distanceToHub);
 
-                return (timeUntilActive) <= timeOfFlight; //  0.5s buffer
+                return (timeUntilActive) <= timeOfFlight;
               })
-          .and(activeHubTrigger.negate())
-          .and(inAllianceZoneTrigger);
+          .and(activeHubTrigger.negate()) // only need to preshoot if not already active
+          .and(inAllianceZoneTrigger) // only preshoot if in alliance zone
+          .and(tooCloseToHubTrigger.negate()); // only shoot if we are far enough away
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump("To Middle"));
-    NamedCommands.registerCommand(
-        "Drive Over Bump To Alliance", swerve.driveOverBump("To Alliance"));
+    NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump(true));
+    NamedCommands.registerCommand("Drive Over Bump To Alliance", swerve.driveOverBump(false));
     NamedCommands.registerCommand("Move To Fuel", new MoveToFuel(swerve).withTimeout(2));
     NamedCommands.registerCommand(
         "Shoot On The Move",
@@ -161,9 +154,9 @@ public RobotContainer() {
     NamedCommands.registerCommand(
         "Shoot", Commands.run(() -> shooter.setSpeed(MetersPerSecond.of(1))).withTimeout(1));
 
-    // Configure the trigger bindings
     configureDriverBindings();
     // configureOperatorBindings();
+
     swerve.configureAutoBuilder();
 
     configureAutoChooser();
@@ -196,15 +189,6 @@ public RobotContainer() {
     activeHubTrigger.onFalse(Commands.runOnce(() -> canPreShoot = false));
   }
 
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
   private void configureFuelSim() {
     fuelInstance.spawnStartingFuel();
     fuelInstance.registerRobot(
@@ -237,6 +221,7 @@ private void configureFuelSim() {
 
     SmartDashboard.putBoolean("Air Resistance Toggle", false);
     SmartDashboard.putBoolean("Only Score while Active", false);
+    SmartDashboard.putBoolean("Automated Shooting Toggle", false);
   }
 
   private void configureDriverBindings() {
@@ -247,6 +232,8 @@ private void configureDriverBindings() {
     (activeHubTrigger.or(() -> canPreShoot))
         .and(automatedShootingTrigger)
         .and(inAllianceZoneTrigger)
+        .and(shootButton.negate())
+        .and(tooCloseToHubTrigger.negate())
         .whileTrue(
             new ShootOnTheMove(
                 swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
@@ -268,16 +255,7 @@ private void configureDriverBindings() {
     turret.setDefaultCommand(turret.faceTarget(goalShotTargetSupplier, swerve::getRobotPose));
 
     hood.setDefaultCommand(hood.aimForTarget(goalShotTargetSupplier, swerve::getRobotPose));
-    // hood.setDefaultCommand(
-    //     new PhysicsStationaryShoot(
-    //         shooter,
-    //         hood,
-    //         robotVisualization,
-    //         swerve::getRobotPose,
-    //         AllianceUtil::getHubPose,
-    //         () -> FieldConstants.mainHubHeight));
-
-    // shooter.setDefaulyCommand();
+
     shootButton.whileTrue(
         new ShootOnTheMove(
             swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
@@ -287,6 +265,7 @@ private void configureOperatorBindings() {
     (activeHubTrigger.or(() -> canPreShoot))
         .and(automatedShootingTrigger)
         .and(inAllianceZoneTrigger)
+        // .and(shootButton.negate())
         .whileTrue(
             new ShootOnTheMove(
                 swerve, turret, hood, shooter, goalShotTargetSupplier, robotVisualization));
@@ -332,7 +311,7 @@ private void configureAutoChooser() {
     autoChooser.addOption(
         "[SysID] Quasistatic Steer Forward", swerve.sysIdQuasistaticSteer(Direction.kForward));
     autoChooser.addOption(
-        "[SysID] Quasistatic Steer Reverse", swerve.sysIdQuasistaticSteer(Direction.kForward));
+        "[SysID] Quasistatic Steer Reverse", swerve.sysIdQuasistaticSteer(Direction.kReverse));
     autoChooser.addOption(
         "[SysID] Dynamic Steer Forward", swerve.sysIdDynamicSteer(Direction.kForward));
     autoChooser.addOption(

src/main/java/frc/robot/commands/GuidedTeleopSwerve.java

@@ -109,9 +109,8 @@ private double getDotProduct() {
     commandedVelocity.set(1, 0, getStrafeSpeed());
 
     Vector<N2> toTarget = new Vector<>(Nat.N2());
-
-    toTarget.set(0, 0, robotPose.getX() - targetPose.getX());
-    toTarget.set(1, 0, robotPose.getY() - targetPose.getY());
+    toTarget.set(0, 0, (flipFactor) * (targetPose.getX() - robotPose.getX()));
+    toTarget.set(1, 0, (flipFactor) * (targetPose.getY() - robotPose.getY()));
 
     double dotProduct = commandedVelocity.unit().dot(toTarget.unit());
 
@@ -185,7 +184,7 @@ private void driveBump(double forward, double strafe) {
     swerve.setControl(
         fieldOriented
             .withVelocityX(forward)
-             .withVelocityY(strafe)
+            .withVelocityY(strafe)
             .withRotationalRate(rotDiagonalSpeed));
   }
 
@@ -204,7 +203,8 @@ public void execute() {
     double dotProduct = currentDriveMode == DriveMode.NormalDrive ? 0 : getDotProduct();
 
     DriveMode effectiveDriveMode =
-        (manualOverrideSupplier.getAsBoolean() || (dotProduct < .353))
+        (manualOverrideSupplier.getAsBoolean()
+                || (dotProduct < 0.05)) // only lock if in trying to drive in that direction
             ? DriveMode.NormalDrive
             : currentDriveMode;
 

src/main/java/frc/robot/commands/ShootOnTheMove.java

@@ -95,29 +95,31 @@ public void execute() {
             swerve, turret, targetPoseSupplier.get(), fieldAccelX, fieldAccelY, fieldSpeeds);
 
     turret.setTargetAngle(shootingParameters.turretAngle());
-
     hood.setTargetAngle(shootingParameters.hoodAngle());
     shooter.setGoalSpeed(shootingParameters.shooterSpeed());
+    swerve.setLookAheadPose(shootingParameters.lookAheadPosition());
 
     double turretErrorDeg =
         turret.getTurretAngle().in(Degrees) - shootingParameters.turretAngle().in(Degrees);
     double hoodErrorDeg =
         hood.getHoodAngle().in(Degrees) - shootingParameters.hoodAngle().in(Degrees);
+
     if (turretSetPointDebouncer.calculate(Math.abs(turretErrorDeg) <= turretTolerance)
         && hoodSetPointDebouncer.calculate(Math.abs(hoodErrorDeg) <= hoodTolerance)
         && shooterDebouncer.calculate(shooterAtSetPoint)) {
       if (isFirstShot
           || ((Timer.getFPGATimestamp() - startTime) > 1 / SimConstants.fuelsPerSecond)) {
         robotVisualization.shootFuel(shootingParameters);
+
         startTime = Timer.getFPGATimestamp();
         isFirstShot = false;
       }
+
     } else {
       // indexer.stop();
     }
   }
 
-  // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     // shooter.stop();
@@ -126,7 +128,6 @@ public void end(boolean interrupted) {
     hood.stopHood();
   }
 
-  // Returns true when the command should end.
   @Override
   public boolean isFinished() {
     return false;

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

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.SpindexerConstants;
@@ -16,6 +17,7 @@ public class Spindexer extends SubsystemBase {
   private TalonFX SpindexerMotor;
   private LaserCan SpindexerLaser;
   private Debouncer SpindexDebouncer;
+  private Debouncer currentEmptyDebouncer = new Debouncer(0.4);
 
   /** Creates a new Spindexer. */
   public Spindexer() {
@@ -43,6 +45,14 @@ public double getSpeed() {
     return SpindexerMotor.get();
   }
 
+  public double getCurrent() {
+    return SpindexerMotor.getStatorCurrent().getValueAsDouble();
+  }
+
+  public boolean currentSaysEmpty() {
+    return currentEmptyDebouncer.calculate(getCurrent() < 9.0); // random number need to test
+  }
+
   public Command runSpindexer() {
     return run(this::setSpeed);
   }
@@ -65,10 +75,6 @@ public Command downSpeed(double speed) {
         });
   }
 
-  // new command runUntilEMpty
-  // empty means laser can cant see anything anymore
-  // look up debouncer
-
   public Command runUntilEmptyCommand() {
     return (runSpindexer()).until(() -> SpindexDebouncer.calculate(!beamBroken()));
   }
@@ -88,8 +94,13 @@ public boolean beamBroken() {
     }
   }
 
+  public boolean isEmpty() {
+    return !beamBroken() && currentSaysEmpty();
+  }
+
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    SmartDashboard.putNumber("Spindexer Current", getCurrent());
+    SmartDashboard.putBoolean("Spindexer Beam Broken", beamBroken());
   }
 }

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

@@ -49,6 +49,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.SimConstants;
 import frc.robot.Constants.TurretConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
@@ -63,6 +64,7 @@
 import java.util.Optional;
 import java.util.Set;
 import java.util.function.Supplier;
+import lombok.Setter;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -108,8 +110,6 @@ public int compareTo(PoseEstimate other) {
           .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)
           .withSteerRequestType(SteerRequestType.Position);
 
-  private Timer timer = new Timer();
-
   private double targetFuelYaw = 0;
   public Rotation2d desiredFuelRotation = Rotation2d.k180deg;
 
@@ -146,6 +146,8 @@ public int compareTo(PoseEstimate other) {
 
   public SwerveDriveState stateCache = getState();
 
+  @Setter private Translation2d lookAheadPose = new Translation2d();
+
   private ExtendedVisionSystemSim visionSim;
 
   private PhotonCameraSim arducamLeftSim;
@@ -348,20 +350,23 @@ public Command pathFindToPose(Pose2d targetPose) {
         Set.of(this));
   }
 
-  public Command driveOverBump(String direction) {
-    return Commands.runOnce(timer::restart)
-        .andThen(
-            Commands.run(
-                () -> {
-                  if (direction == "To Middle") {
-                    this.setControl(
-                        fieldOriented.withVelocityX(0.1).withVelocityY(0).withRotationalRate(0));
-                  } else {
-                    this.setControl(
-                        fieldOriented.withVelocityX(-0.1).withVelocityY(0).withRotationalRate(0));
-                  }
-                }))
-        .until(() -> timer.hasElapsed(0.5) && getPigeon2().getPitch().getValueAsDouble() == 0);
+  public Command driveOverBump(boolean toMiddle) {
+    double startTime = Timer.getFPGATimestamp();
+    return Commands.run(
+            () -> {
+              if (toMiddle) {
+                this.setControl(
+                    fieldOriented.withVelocityX(0.1).withVelocityY(0).withRotationalRate(0));
+              } else {
+                this.setControl(
+                    fieldOriented.withVelocityX(-0.1).withVelocityY(0).withRotationalRate(0));
+              }
+            })
+        .until(
+            () ->
+                (Timer.getFPGATimestamp() - startTime) > 0.5
+                    && Math.abs(getPigeon2().getPitch().getValueAsDouble()) <= 0.353)
+        .withTimeout(Seconds.of(3.53));
   }
 
   public Command pathFindThroughTrench() {
@@ -396,6 +401,21 @@ public Command pathFindThroughTrench() {
         .withName("PathFindThroughTrench");
   }
 
+  public boolean tooCloseToHub() {
+
+    Translation2d turretPose =
+        stateCache.Pose
+            .getTranslation()
+            .plus(
+                TurretConstants.robotToTurretTransform
+                    .getTranslation()
+                    .rotateBy(stateCache.Pose.getRotation()));
+
+    double turretToHubDistance = lookAheadPose.getDistance(turretPose);
+
+    return turretToHubDistance < SimConstants.closestPossibleShotDistance.in(Meters);
+  }
+
   public boolean onLeftSide() {
     Pose2d pose = stateCache.Pose;
     return !AllianceUtil.isRedAlliance() && pose.getMeasureY().gt(FieldConstants.fieldWidth.div(2))