added new command, YIPPIE!

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

@@ -21,6 +21,7 @@
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 
+import frc.robot.commands.AimAtAmp;
 import frc.robot.commands.AimRobotMoving;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.Autos;
@@ -167,7 +168,8 @@ private void driveTrainInst() {
   }
   private void shooterInst() {
     shooter = new ShooterSubsystem(ShooterConstants.SHOOTER_MOTOR_POWER);
-    angleShooterSubsystem = new AngleShooterSubsystem(()-> (operatorController.getPOV() == 0));
+    angleShooterSubsystem = new AngleShooterSubsystem();
+    angleShooterSubsystem.setDefaultCommand(new AimAtAmp(angleShooterSubsystem, ()-> (operatorController.getPOV() == 0)));
   }
   private void intakeInst() {
     intake = new IntakeSubsystem();

src/main/java/frc/robot/commands/AimAtAmp.java

@@ -0,0 +1,45 @@
+package frc.robot.commands;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.settings.Constants.Field;
+import frc.robot.settings.Constants.ShooterConstants;
+import frc.robot.subsystems.AngleShooterSubsystem;
+
+public class AimAtAmp extends Command {
+	AngleShooterSubsystem angleShooterSubsystem;
+	BooleanSupplier aimAtAmp;
+
+	public AimAtAmp(AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier aimAtAmp) {
+		this.angleShooterSubsystem = angleShooterSubsystem;
+		this.aimAtAmp = aimAtAmp;
+	}
+
+	@Override
+	public void initialize() {
+
+	}
+
+	@Override
+	public void execute() {
+		if (!aimAtAmp.getAsBoolean()) {
+			double desiredShooterAngleSpeed = angleShooterSubsystem.calculateSpeakerAngle() * ShooterConstants.AUTO_AIM_SHOOTER_kP;
+			angleShooterSubsystem.pitchShooter(desiredShooterAngleSpeed);
+		} else {
+			double differenceAmp = Field.AMPLIFIER_ANGLE - angleShooterSubsystem.getShooterAngle();
+			double ampSpeed = differenceAmp * ShooterConstants.AUTO_AIM_SHOOTER_kP;
+			angleShooterSubsystem.pitchShooter(ampSpeed);
+		}
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		angleShooterSubsystem.pitchShooter(0);
+	}
+}

src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java

@@ -24,13 +24,11 @@ public class AngleShooterSubsystem extends SubsystemBase {
 	CANSparkMax pitchMotor;
 	SparkPIDController pitchPID;
 	CANcoder angleEncoder;
-	BooleanSupplier aimAtAmp;
 	double shootingSpeed = ShooterConstants.SHOOTING_SPEED_MPS;
 	public static Pose2d dtvalues;
 	public static ChassisSpeeds DTChassisSpeeds;
 
-	public AngleShooterSubsystem(BooleanSupplier aimAtAmp) {
-		this.aimAtAmp = aimAtAmp;
+	public AngleShooterSubsystem() {
 		pitchMotor = new CANSparkMax(ShooterConstants.PITCH_MOTOR_ID, MotorType.kBrushless);
 
 		pitchPID = pitchMotor.getPIDController();
@@ -105,15 +103,5 @@ public double calculateSpeakerAngle() {
 		return differenceAngle;
 	}
 
-	@Override
-	public void periodic() {
-		if (!aimAtAmp.getAsBoolean()) {
-			double desiredShooterAngleSpeed = calculateSpeakerAngle() * ShooterConstants.AUTO_AIM_SHOOTER_kP;
-			pitchShooter(desiredShooterAngleSpeed);
-		} else {
-			double differenceAmp = Field.AMPLIFIER_ANGLE - this.getShooterAngle();
-			double ampSpeed = differenceAmp * ShooterConstants.AUTO_AIM_SHOOTER_kP;
-			pitchShooter(ampSpeed);
-		}
-	}
+
 }