diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java index 1e419cda..29e711c4 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java @@ -1,5 +1,27 @@ package frc.robot.commands.shooter.pivot; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; -public class ShooterPivotVerticalCommand { - -} +public class ShooterPivotVerticalCommand extends Command{ + ShooterPivotSubsystem pivotSubsystem; + + public ShooterPivotVerticalCommand(ShooterPivotSubsystem pivotSubsystem){ + this.pivotSubsystem = pivotSubsystem; + addRequirements(pivotSubsystem); + } + + @Override + public void initialize() { + pivotSubsystem.setAutoAimBoolean(false); + pivotSubsystem.setAngle(90.0); + } + + @Override + public boolean isFinished() { + if(Math.abs(pivotSubsystem.getPosition()) - 90 < pivotSubsystem.ERRORTOLERANCE){ + return true; + } + + return false; + } +} \ No newline at end of file