more work on task test system and minor improvements

src/main/java/frc/robot/Constants.java

@@ -13,5 +13,6 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static double[] servoPresets={20, 90, 10, 50};
+    public static final double[] servoPresets={20, 90, 10, 50};
+    public static final double servoDegreesPerSecond=600;
 }

src/main/java/frc/robot/Robot.java

@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.DriveCommands;
+import frc.robot.subsystems.Rangefinder;
 import frc.robot.subsystems.Servo;
 import frc.robot.subsystems.XRPDrivetrain;
 
@@ -31,6 +32,7 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
   private SendableChooser<Integer> m_chooser = new SendableChooser<>();
+  private boolean showingSensorDistance = false;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -78,9 +80,9 @@ public void disabledPeriodic() {}
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
-    }
+    // if (m_autonomousCommand != null) {
+    //   m_autonomousCommand.schedule();
+    // }
     switch (m_chooser.getSelected()) {
       case 1:
         SmartDashboard.putString("What should happen: ", "The XRP should turn about 90 degrees(don't worry if it's not accurate!)");
@@ -151,7 +153,7 @@ public void autonomousInit() {
         } catch (NoSuchMethodException e) {
           // TODO Auto-generated catch block
           try {
-            ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).newInstance()).schedule();
+            ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).newInstance(5)).schedule();
           } catch (ClassNotFoundException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException | SecurityException e1) {
             // TODO Auto-generated catch block
             e1.printStackTrace();
@@ -178,17 +180,33 @@ public void autonomousInit() {
         break;
       case 6:
         SmartDashboard.putString("What should happen: ", "The XRP should move its servo to each of the presets.");
-        try{
-
-        }
         break;
       case 7:
+        SmartDashboard.putString("What should happen: ", "Change the mode from Auto to Teleop. You should now be able to control the XRP's servo!");
         break;
-        case 8:
+      case 8:
+        SmartDashboard.putString("What should happen: ", "You should now see the distance the rangefinder reads!");
+        showingSensorDistance=true;
         break;
       case 9:
+        SmartDashboard.putString("What should happen: ", "The XRP should execute the command and move until an object in front of it is less than 2 inches away.");
+        try {
+          DriveCommands.class.getMethod("driveToWall").invoke(null);
+        } catch (NoSuchMethodException e) {
+          // TODO Auto-generated catch block
+          try {
+            ((Command) Class.forName("DriveCommands$DriveToWall").getDeclaredConstructor().newInstance()).schedule();
+          } catch (ClassNotFoundException | InstantiationException | IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException | SecurityException e1) {
+            // TODO Auto-generated catch block
+            e1.printStackTrace();
+          }
+        } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException e) {
+          // TODO Auto-generated catch block
+          e.printStackTrace();
+        }
         break;
       case 10:
+        SmartDashboard.putString("What should happen: ", "Change the mode from Auto to Teleop. You should now be able to control the XRP using a tank drive scheme!");
         break;
       case 11:
         break;
@@ -200,7 +218,16 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+    if(showingSensorDistance)
+      try {
+        SmartDashboard.putNumber("Rangefinder Distance", (double) Rangefinder.class.getMethod("getDistance").invoke(RobotContainer.m_rangefinder));
+      } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException
+          | SecurityException e) {
+        // TODO Auto-generated catch block
+        e.printStackTrace();
+      }
+  }
 
   @Override
   public void teleopInit() {

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

@@ -229,10 +229,8 @@ 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.
-
+  //is less than 2 inches. Name it driveToWall or DriveToWall.
   //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.
+  //and move the XRP forward 3 inches while moving the arm to 0 degrees using a by Parallel Command group.
   //HINT: Parallel Command Groups are written the same way as Sequential Command Groups.
 }

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

@@ -4,5 +4,9 @@
 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.
     //Name them servoUpCommand, servoDownCommand, and servoPresetCommand. The presets are in the constants file.
+    //NOTE: To make servoPresetCommand work properly with the Command system, you will need to make a non-Instant Command, one that ends when the servo's movement is complete.
+    //However, because the library is programmed strangely, you only get the angle you set the servo to, not the actual angle it is at during any point in time. (ie if you set the servo to move to 90 degrees from 0 degrees and you called getAngle immediately after, before any movement occurs, you simply get the value 90)
+    //The speed the servo moves is stored in Constants. Use it and System.currentTimeMillis() to determine how long the servo will move for, and set the end condition to be
+    //when the time elapsed is greater than or equal to the time the servo moves.
 
 }

src/main/java/frc/robot/subsystems/Rangefinder.java

@@ -9,6 +9,6 @@ public class Rangefinder extends SubsystemBase{
     public Rangefinder(){
         m_rangefinder=new XRPRangefinder();
     }
-    //TODO: Task 8-Write a method to get the distance returned by the sensor in inches.
+    //TODO: Task 8-Write a method to get the distance returned by the sensor in inches. Name it getDistance
     //HINT: Type 'm_rangefinder.' (without quotes) in a method body to see all of the different methods the rangefinder has.
 }

src/main/java/frc/robot/subsystems/Servo.java

@@ -9,7 +9,6 @@ public class Servo extends SubsystemBase {
     public Servo(){
         // Device number 4 maps to the physical Servo 1 port on the XRP
         m_armServo = new XRPServo(4);
-
     }
     //TODO: Task 5-Write functions to control the arm. You should be able to set the arm's angle and get the angle(double)
     //it is at. The functions should be called setServoAngle and getServoAngle