From e4e63f383069bce058695dbd7b709c878b86dc88 Mon Sep 17 00:00:00 2001
From: Saanvi Korem <saanvikorem@gmail.com>
Date: Wed, 23 Oct 2024 23:49:06 -0400
Subject: [PATCH] Task 6 Completion

---
 .../frc/robot/commands/ServoCommands.java     | 21 ++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/ServoCommands.java b/src/main/java/frc/robot/commands/ServoCommands.java
index a924e0d..0da19de 100644
--- a/src/main/java/frc/robot/commands/ServoCommands.java
+++ b/src/main/java/frc/robot/commands/ServoCommands.java
@@ -4,5 +4,24 @@
 public class ServoCommands {
     //TODO: Task 6-Write commands to move the arm up, down, and to a specific preset. The latter should be done by taking in an integer input.
     //The presets are in the constants file.
-    
+    public static InstantCommand moveArmUp(integer numberr){
+        return new InstantCommand()->{
+            double angleNew = servo.getArmAngle() + numberr;
+            servo.setArmAngle(angleNew);
+        }
+    }
+    public static InstantCommand moveArmDown(integer numberr){
+        return new InstantCommand () -> {
+            double angleNew = servo.getArmAngle() - numberr;
+            servo.setArmAngle(angleNew);
+        }
+    }
+    public static InstantCommand presetArm(int presetI){
+        return new InstantCommand () -> {
+            if(presetI < Constants.servoPresets.length && presetI >=0){
+                double AngleA = Constants.servoPresets[presetI];
+                servo.setArmAngle(AngleA);
+            }
+        }
+    }
 }