commands

grt192 · Feb 2, 2024 · 71418ac · 71418ac
1 parent c9638d3
commit 71418ac
Showing 1 changed file with 25 additions and 3 deletions.
diff --git 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;
+    }
+}