added safe auto aim and updated auto aim shot point (red side)

Team334 · Mar 14, 2024 · 59c2083 · 59c2083
1 parent ef7c9f7
commit 59c2083
Show file tree

Hide file tree

Showing 3 changed files with 57 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -161,7 +161,7 @@ public static class FieldConstants {
 
     // x: .2472 y: 5.5946 (USE THIS)
     public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.17, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180));
-    public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.8, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here
+    public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.4, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here
   }
 
   public static class LEDColors {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -164,18 +165,30 @@ private void configureBindings() {
     // );
 
     // driver bindings
+    _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
-    _driveController.L1().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetPose(new Pose2d()), _swerveSubsystem));
-    _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
-    _driveController.L2().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
-
     _driveController.triangle().onTrue(Commands.runOnce(_swerveSubsystem::resetGyro, _swerveSubsystem));
+    _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
+
+    _driveController.L2().whileTrue(
+      new AutoAim(
+        _shooterSubsystem, 
+        _elevatorSubsystem, 
+        _ledSubsystem, 
+        _swerveSubsystem,
+        () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
+        () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05),
+        Presets.CLOSE_SHOOTER_ANGLE, 
+        Presets.CLOSE_ELEVATOR_HEIGHT,
+        () -> (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180
+      )
+    );
 
     _driveController.R2().whileTrue(
       new AutoAim(
         _shooterSubsystem,
         _elevatorSubsystem,
-        null,
+        _ledSubsystem,
         _swerveSubsystem,
         () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
         () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05)

diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim.java b/src/main/java/frc/robot/commands/shooter/AutoAim.java
@@ -88,6 +88,44 @@ public AutoAim(
     addRequirements(_swerve, _shooter, _elevator);
   }
 
+  /**
+   * Constructs an AutoAim that does NOT finish when reaching setpoints.
+   * It DOES NOT calculate angle/height/heading setpoints.
+   * 
+   * (USE FOR TELEOP)
+   * 
+   * @param shooter Shooter subsystem.
+   * @param elevator Elevator subsystem.
+   * @param leds Led subsystem.
+   * @param swerve Swerve subsystem.
+   * @param xSpeed X joystick speed.
+   * @param ySpeed Y joystick speed.
+   * 
+   * @param shooterAngle Desired shooter angle.
+   * @param elevatorHeight Desired elevator height.
+   * @param swerveHeadingSupplier Desired swerve heading supplier. This is a supplier since heading could change based on alliance.
+   */
+  public AutoAim(
+    ShooterSubsystem shooter, 
+    ElevatorSubsystem elevator, 
+    LEDSubsystem leds, 
+    SwerveDriveSubsystem swerve,
+    DoubleSupplier xSpeed,
+    DoubleSupplier ySpeed,
+    double shooterAngle,
+    double elevatorHeight,
+    DoubleSupplier swerveHeadingSupplier
+  ) {
+    this(shooter, elevator, leds, swerve, xSpeed, ySpeed);
+
+    _desiredShooterAngle = shooterAngle;
+    _desiredElevatorHeight = elevatorHeight;
+    _swerveHeadingSupplier = swerveHeadingSupplier;
+
+    _overrideDesired = true;
+  }
+
+
   /**
    * Constructs an AutoAim that finishes when it reaches its initial setpoints.
    * It calculates angle/height/heading setpoints.