added swerve into autoaim2

Team334 · Mar 19, 2024 · 20a0232 · 20a0232
1 parent bc1873e
commit 20a0232
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/commands/shooter/AutoAim2.java b/src/main/java/frc/robot/commands/shooter/AutoAim2.java
@@ -69,7 +69,7 @@ public AutoAim2(
     DoubleSupplier xSpeed,
     DoubleSupplier ySpeed
   ) {
-    this(swerve, shooter, elevator, xSpeed, ySpeed, () -> 0, shooter::speakerAngle, elevator::speakerHeight, false);
+    this(swerve, shooter, elevator, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, false);
   }
 
   /** 
@@ -100,7 +100,7 @@ public AutoAim2(
     ShooterSubsystem shooter,
     ElevatorSubsystem elevator
   ) {
-    this(swerve, shooter, elevator, () -> 0, () -> 0, () -> 0, shooter::speakerAngle, elevator::speakerHeight, true);
+    this(swerve, shooter, elevator, () -> 0, () -> 0, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, true);
   }
 
   /** 

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -359,6 +359,15 @@ public double[] speakerSetpoints() {
     return offsets;
   }
 
+  /**
+   * Get the calculated heading to aim the chassis at the speaker shot point.
+   */
+  public double speakerHeading() {
+    Translation2d distance = shotVector();
+
+    return 0;
+  }
+
   public void pivotMotor(Translation2d pivotPoint) {
     _pivotPoint = pivotPoint;
   }