diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c215ba3..d7e76fee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); diff --git a/src/main/java/frc/robot/commands/AimShooter.java b/src/main/java/frc/robot/commands/AimShooter.java new file mode 100644 index 00000000..9452fb3e --- /dev/null +++ b/src/main/java/frc/robot/commands/AimShooter.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 AimShooter extends Command { + AngleShooterSubsystem angleShooterSubsystem; + BooleanSupplier aimAtAmp; + + public AimShooter(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); + } +} diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java index 574a5a9b..2233feb3 100644 --- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java +++ b/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); - } - } + } \ No newline at end of file