Merge pull request #27 from IgniteRobotics/faeture/launch-interface

Re-merging Faeture/launch interface

Author: y0shi

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

@@ -22,6 +22,7 @@
 import frc.robot.subsystems.drive.DrivetrainSubsystem;
 import frc.robot.subsystems.indexer.IndexerSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
 
 @Logged
 public class RobotContainer {
@@ -44,6 +45,9 @@ public class RobotContainer {
   @Logged(name = "Climber")
   public final ClimberSubsystem climber = new ClimberSubsystem();
 
+  @Logged(name = "Shooter")
+  public final ShooterSubsystem shooter = new ShooterSubsystem();
+
   private final SendableChooser<Command> autoChooser;
 
   public RobotContainer() {

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -31,5 +31,4 @@ public class DriveConstants {
 
   public static final double TRANSLATION_ALIGN_KP = 0.0001;
   public static final double ROTATION_ALIGN_KP = 6.28;
-
 }

src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java

@@ -41,6 +41,8 @@ public DrivetrainSubsystem() {
     applyDriveGains();
     configureAutoBuilder();
     configureCANrange();
+    // TODO: remove.  debugging.
+    periodic();
   }
 
   @Override

src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java

@@ -4,29 +4,19 @@
 
 package frc.robot.subsystems.shooter;
 
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
-import java.util.function.Supplier;
 
 /** Add your docs here. */
-public abstract class LaunchRequestBuilder {
+public interface LaunchRequestBuilder {
 
   public record LaunchRequest(
       Angle launchHoodTarget,
       AngularVelocity launchVelocity,
       LinearVelocity driveVelocity,
       Rotation2d driveAngle) {}
 
-  Supplier<Pose3d> targetPose;
-
-  protected LaunchRequestBuilder(Supplier<Pose3d> targetPose) {
-    this.targetPose = targetPose;
-  }
-
-  public abstract LaunchRequestBuilder getInstance(Supplier<Pose3d> targetPose);
-
-  public abstract LaunchRequest createLaunchRequest();
+  public LaunchRequest createLaunchRequest();
 }

src/main/java/frc/robot/subsystems/shooter/MappedLauchRequestBuilder.java

@@ -26,25 +26,14 @@
 import java.util.function.Supplier;
 
 /** Add your docs here. */
-public class MappedLauchRequestBuilder extends LaunchRequestBuilder {
+public class MappedLauchRequestBuilder implements LaunchRequestBuilder {
 
-  private MappedLauchRequestBuilder instance;
-
-  private MappedLauchRequestBuilder(Supplier<Pose3d> targetPose) {
-    super(targetPose);
-  }
+  private Supplier<Pose3d> targetPose;
 
   private LaunchRequest currentLaunchRequest;
 
-  @Override
-  public MappedLauchRequestBuilder getInstance(Supplier<Pose3d> targetPose) {
-    if (this.instance != null) {
-      instance = new MappedLauchRequestBuilder(targetPose);
-      return instance;
-    } else {
-      this.targetPose = targetPose;
-      return instance;
-    }
+  public MappedLauchRequestBuilder(Supplier<Pose3d> targetPose) {
+    this.targetPose = targetPose;
   }
 
   private double loopPeriodSecs = 0.02;
@@ -125,7 +114,6 @@ public MappedLauchRequestBuilder getInstance(Supplier<Pose3d> targetPose) {
     passingTimeOfFlightMap.put(passingMaxDistance, 0.0);
   }
 
-  @Override
   public LaunchRequest createLaunchRequest() {
 
     boolean passing = targetPose.get().getZ() == 0.0; // target pose is the floor, so we're passing.
@@ -180,6 +168,8 @@ public LaunchRequest createLaunchRequest() {
     // calcuate rotation angle
     Rotation2d driveAngle =
         getDriveAngle(lookaheadPose, targetPose.get().getTranslation().toTranslation2d());
+    // if no last drive angle, default to the robot's current pose info.
+    if (lastDriveAngle == null) lastDriveAngle = estimatedRobotPose.getRotation();
 
     // calculate hood angle
     double hoodAngle =

src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java

@@ -14,26 +14,14 @@
 import java.util.function.Supplier;
 
 /** Add your docs here. */
-public class ParabolicLaunchRequestBuilder extends LaunchRequestBuilder {
+public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder {
 
-  private ParabolicLaunchRequestBuilder instance;
+  private Supplier<Pose3d> targetPose;
 
   private ParabolicLaunchRequestBuilder(Supplier<Pose3d> targetPose) {
-    super(targetPose);
+    this.targetPose = targetPose;
   }
 
-  @Override
-  public ParabolicLaunchRequestBuilder getInstance(Supplier<Pose3d> targetPose) {
-    if (this.instance != null) {
-      instance = new ParabolicLaunchRequestBuilder(targetPose);
-      return instance;
-    } else {
-      this.targetPose = targetPose;
-      return instance;
-    }
-  }
-
-  @Override
   public LaunchRequest createLaunchRequest() {
     double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters);
     double x2 =

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -5,6 +5,8 @@
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.controls.DutyCycleOut;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Distance;
@@ -73,4 +75,19 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs()
   public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5);
   public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate
   public static final double OPTIMAL_ENTRY_SLOPE = -1; // TODO: Tune
+
+  // TODO:  verify these!
+  // funnel poses.
+  public static final Pose3d BLUE_TARGET =
+      new Pose3d(
+          Distance.ofBaseUnits(4.623, Meters),
+          Distance.ofBaseUnits(4.041, Meters),
+          Distance.ofBaseUnits(1.435, Meters),
+          Rotation3d.kZero);
+  public static final Pose3d RED_TARGET =
+      new Pose3d(
+          Distance.ofBaseUnits(12.276, Meters),
+          Distance.ofBaseUnits(4.041, Meters),
+          Distance.ofBaseUnits(1.435, Meters),
+          Rotation3d.kZero);
 }

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -22,6 +22,8 @@ public class ShooterSubsystem extends SubsystemBase {
   private final TalonFX flywheelMotor;
   private final TalonFX hoodMotor;
 
+  private final LaunchRequestBuilder launchRequestBuilder;
+
   @Logged(name = "Velocity Target", importance = Importance.CRITICAL)
   private AngularVelocity velocityTarget; // Rotations Per Second
 
@@ -79,12 +81,15 @@ public ShooterSubsystem() {
     hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs());
     hoodTarget = Rotations.of(0);
     hoodControl = new PositionTorqueCurrentFOC(0);
+
+    launchRequestBuilder = new MappedLauchRequestBuilder(() -> ShooterConstants.BLUE_TARGET);
   }
 
   @Override
   public void periodic() {
     flywheelMotor.setControl(velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond)));
     hoodMotor.setControl(hoodControl.withVelocity(hoodTarget.in(Rotations)));
+    launchRequestBuilder.createLaunchRequest();
   }
 
   public Command spinFlywheelCommand() {