Task 1 Completion

RoboLoCo-5338 · Oct 17, 2024 · bbb970f · bbb970f
1 parent c810b1a
commit bbb970f
Showing 1 changed file with 18 additions and 0 deletions.
diff --git 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)