Task 9 Completion, also changed some stuff in ServoCommands

src/main/java/frc/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import frc.robot.subsystems.Servo;
 import frc.robot.subsystems.XRPDrivetrain;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -67,7 +68,15 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // The following command will run during autonomous
-    return null;
+    return new FunctionalCommand(
+      () -> {
+        RobotContainer.m_xrpDrivetrain.tankDrive(0, 0);
+        RobotContainer.m_xrpDrivetrain.resetEncoders();
+      }, 
+      () -> RobotContainer.m_xrpDrivetrain.tankDrive(1, 1),
+      interrupted -> RobotContainer.m_xrpDrivetrain.tankDrive(0, 0),
+      () -> (RobotContainer.m_rangefinder.findRange()) <= 2.0,
+      RobotContainer.m_xrpDrivetrain, RobotContainer.m_rangefinder
+    );
   }
 }

src/main/java/frc/robot/commands/DriveCommands.java

@@ -5,11 +5,14 @@
 
 package frc.robot.commands;
 
+import frc.robot.Robot;
 import frc.robot.RobotContainer;
+import frc.robot.subsystems.Rangefinder;
 import frc.robot.subsystems.XRPDrivetrain;
 
 import java.util.function.Supplier;
 
+import edu.wpi.first.wpilibj.xrp.XRPRangefinder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -291,7 +294,19 @@ public static Command sequentialExampleCommand(){
 
   //TODO: Task 9-Write a Command(function or class) that causes the XRP to drive until the distance returned by the rangefinder
   //is less than 2 inches. Afterwards, test it by replacing the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote.
-
+  public static Command driveUntil(){
+    //Rangefinder abc = new Rangefinder();
+    return new FunctionalCommand(
+      () -> {
+        RobotContainer.m_xrpDrivetrain.tankDrive(0, 0);
+        RobotContainer.m_xrpDrivetrain.resetEncoders();
+      }, 
+      () -> RobotContainer.m_xrpDrivetrain.tankDrive(1, 1),
+      interrupted -> RobotContainer.m_xrpDrivetrain.tankDrive(0, 0),
+      () -> (RobotContainer.m_rangefinder.findRange()) <= 2.0,
+      RobotContainer.m_xrpDrivetrain, RobotContainer.m_rangefinder
+    );
+  }
   //TODO: Task 10-Write a Sequential Command Group to move the XRP backwards 2 inches(you may need to make new Commands or alter existing Commands for this), set the arm preset to index 1, spin the XRP 360 degrees
   //and move the XRP forward 3 inches while moving the arm to 0 degrees using a by Parallel Command group. Afterwards, test it by replacing 
   //the return value of getAutonomousCommand() in RobotContainer and setting it to instead return the command you wrote.

src/main/java/frc/robot/commands/ServoCommands.java

@@ -18,28 +18,28 @@
 
 public class ServoCommands {
     //private XRPServo test;
-    private static Servo hiIhopeThisWorks = new Servo();
+    //private static Servo hiIhopeThisWorks = new Servo();
     private static Constants constants = new Constants();
 
     //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 Command servoAdjustAngleCommand(double adjustAmount) {
         return new RunCommand(
-            () -> hiIhopeThisWorks.adjustAngleBy(adjustAmount)
-            //() -> frc.robot.subsystems.Servo.m_armServo.adjustAngleBy(5)
+            () -> RobotContainer.m_servo.adjustAngleBy(adjustAmount)
+            //() -> frc.robot.subsystems.Servo.m_armServo.adjustAngleBy() or maybe RobotContainer.m_servo
             //note to self: if hiIhopeThisWorks doesn't work, then do ^^^ and change m_armServo to public
         );
     }
 
     public static Command servoSetAngleCommand(double angle) {
         return new InstantCommand(
-            () -> hiIhopeThisWorks.setAngle(angle)
+            () -> RobotContainer.m_servo.setAngle(angle)
         );
     }
 
     public static Command servoPresetCommand(int index) {
         return new InstantCommand(
-            () -> hiIhopeThisWorks.setAngle(constants.servoPresets[index])
+            () -> RobotContainer.m_servo.setAngle(constants.servoPresets[index])
         );
     }
 }