Added Goal and Trench Position Commands

Untested

src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java

@@ -8,13 +8,22 @@
 package frc4388.robot.commands.shooter;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
 
 public class ShooterTrenchPosition extends CommandBase {
+  Shooter m_shooter;
+  ShooterHood m_hood;
+  ShooterAim m_aim;
   /**
    * Creates a new ShooterTrenchPosition.
    */
-  public ShooterTrenchPosition() {
-    // Use addRequirements() here to declare subsystem dependencies.
+  public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) {
+    m_shooter = shooterSub;
+    m_hood = hoodSub;
+    m_aim = aimSub;
+    addRequirements(m_shooter,m_hood,m_aim);
   }
 
   // Called when the command is initially scheduled.
@@ -25,6 +34,9 @@ public class ShooterTrenchPosition extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    m_shooter.runDrumShooterVelocityPID(5000);
+    m_hood.runAngleAdjustPID(3);
+    m_aim.runshooterRotatePID(-26.5);
   }
 
   // Called once the command ends or is interrupted.