set custom angle command

grt192 · Feb 2, 2024 · 17abb8b · 17abb8b
1 parent 71418ac
commit 17abb8b
Showing 1 changed file with 29 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java
@@ -0,0 +1,29 @@
+package frc.robot.commands.shooter.pivot;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.shooter.ShooterPivotSubsystem;
+
+public class ShooterPivotSetAngle extends Command{
+    ShooterPivotSubsystem pivotSubsystem;
+    double angle;
+
+    public ShooterPivotSetAngle(ShooterPivotSubsystem pivotSubsystem, double angle){
+        this.pivotSubsystem = pivotSubsystem;
+        addRequirements(pivotSubsystem);
+        this.angle = angle;
+    }
+
+    @Override
+    public void initialize() {
+        pivotSubsystem.setAutoAimBoolean(false);
+        pivotSubsystem.setAngle(angle);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if(Math.abs(pivotSubsystem.getPosition()) - angle < pivotSubsystem.ERRORTOLERANCE){
+            return true;
+        }
+
+        return false;
+    }
+}