From bbb970f8507e8d388e626bdae01ececed0bd9ac7 Mon Sep 17 00:00:00 2001
From: NakulS <875605@lcps.org>
Date: Thu, 17 Oct 2024 17:47:10 -0400
Subject: [PATCH] Task 1 Completion

---
 .../java/frc/robot/commands/DriveCommands.java | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java
index daed6c1..5bd047c 100644
--- a/src/main/java/frc/robot/commands/DriveCommands.java
+++ b/src/main/java/frc/robot/commands/DriveCommands.java
@@ -199,7 +199,25 @@ has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm
 
   //TODO: Task 1-Rewrite TurnDegrees as a function that returns a functional command
   //Code here:
+  public static Command TurnDegrees(double speed, double degrees){
+    return new FunctionalCommand(
+      () -> {
+        RobotContainer.m_xrpDrivetrain.arcadeDrive(0, 0); 
+        RobotContainer.m_xrpDrivetrain.resetEncoders();
+      }, 
+
+      () -> RobotContainer.m_xrpDrivetrain.arcadeDrive(0, speed),
 
+      interrupted -> RobotContainer.m_xrpDrivetrain.arcadeDrive(0, 0),
+
+      () -> {
+        double inchPerDegree = Math.PI * 6.102 / 360;
+        return RobotContainer.m_xrpDrivetrain.getAverageTurningDistance() >= (inchPerDegree * degrees);
+      },
+
+      RobotContainer.m_xrpDrivetrain
+      );
+  }
   //TODO: Task 3-Rewrite the following functions to use tank drive
   //driveDistance or AltDriveDistance(choose 1)
   //arcadeDriveCommand(rename as tankDriveCommand)