add low feed shot as a superstructure command

team4909 · Apr 15, 2024 · 1dbf78d · 1dbf78d
1 parent 60dd55f
commit 1dbf78d
Show file tree

Hide file tree

Showing 5 changed files with 29 additions and 16 deletions.
diff --git a/src/main/java/frc/robot/PoseEstimation.java b/src/main/java/frc/robot/PoseEstimation.java
@@ -154,6 +154,14 @@ public AimingParameters getAimingParameters() {
     return m_lastAimingParameters;
   }
 
+  public Rotation2d getFeedHeading() {
+    return FieldConstants.stashPositionSupplier
+        .get()
+        .minus(getPose().getTranslation())
+        .getAngle()
+        .rotateBy(new Rotation2d(Math.PI));
+  }
+
   private Pose2d poseInverse(Pose2d pose) {
     Rotation2d rotationInverse = pose.getRotation().unaryMinus();
     return new Pose2d(

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -231,7 +231,7 @@ public Robot() {
     m_driverController
         .leftTrigger()
         .and(m_drivetrain.inRangeOfGoal.negate())
-        .whileTrue(Superstructure.aimAtStash(m_drivetrain, m_shooter, m_arm, m_lights))
+        .whileTrue(Superstructure.feedShotHigh(m_drivetrain, m_shooter, m_arm, m_lights))
         .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed));
 
     m_driverController.start().onTrue(m_drivetrain.zeroGyro());
@@ -245,7 +245,7 @@ public Robot() {
     m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb));
     m_operatorController
         .rightStick()
-        .whileTrue(m_arm.goToSetpoint(ArmSetpoints.kStash))
+        .whileTrue(m_arm.goToSetpoint(ArmSetpoints.kFeedLow))
         .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed));
     m_driverController.b().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap));
     m_driverController.b().whileTrue(Commands.parallel(m_climber.windWinch()));

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
@@ -1,6 +1,5 @@
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.util.Color;
@@ -65,22 +64,28 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm,
         .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
   }
 
-  public static Command aimAtStash(Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) {
+  public static Command feedShotHigh(
+      Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) {
     return Commands.parallel(
             Commands.startEnd(
-                () ->
-                    drivetrain.setHeadingGoal(
-                        () ->
-                            FieldConstants.stashPositionSupplier
-                                .get()
-                                .minus(PoseEstimation.getInstance().getPose().getTranslation())
-                                .getAngle()
-                                .rotateBy(new Rotation2d(Math.PI))),
+                () -> drivetrain.setHeadingGoal(PoseEstimation.getInstance()::getFeedHeading),
                 drivetrain::clearHeadingGoal),
             shooter.feederShot(),
-            arm.goToSetpoint(ArmSetpoints.kStash),
+            arm.goToSetpoint(ArmSetpoints.kFeedHigh),
             lights.showReadyToShootStatus(
-                drivetrain.atHeadingGoal.and(drivetrain.atHeadingGoal).and(arm.atGoal)))
+                drivetrain.atHeadingGoal.and(arm.atGoal).and(shooter.readyToShoot)))
+        .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
+  }
+
+  public static Command feedShotLow(
+      Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) {
+    return Commands.parallel(
+            Commands.startEnd(
+                () -> drivetrain.setHeadingGoal(PoseEstimation.getInstance()::getFeedHeading),
+                drivetrain::clearHeadingGoal),
+            shooter.runShooter(),
+            arm.goToSetpoint(ArmSetpoints.kFeedLow),
+            lights.showReadyToShootStatus(drivetrain.atHeadingGoal.and(arm.atGoal)))
         .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
   }
 

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -250,7 +250,8 @@ public enum ArmSetpoints {
     kAmp(1.49 + 0.0873 + Units.degreesToRadians(3.0), Units.degreesToRadians(228), 0.0, 0.0),
     kClimb(1.633, -2.371, 0.0, 0.0),
     kTrap(Units.degreesToRadians(53.0), Units.degreesToRadians(80.0), 0.0, 0.0),
-    kStash(Units.degreesToRadians(-31), 2.083, 0.0, 0.0);
+    kFeedHigh(Units.degreesToRadians(-31), 2.083, 0.0, 0.0),
+    kFeedLow(Units.degreesToRadians(10.0), Units.degreesToRadians(170.0), 0.0, 0.0);
 
     public final double elbowAngle;
     public final double wristAngle;

diff --git a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java
@@ -11,7 +11,6 @@
 import java.time.Duration;
 
 public class FeederIOTalonFX implements FeederIO {
-  private final boolean kUseHighFrequencySensorPolling = true;
   private final TalonFX m_feederMotor;
   private final DigitalInput m_noteSensor;
   private final DigitalGlitchFilter m_glitchFilter;