started work on system to test tasks easier

simgui.json

@@ -20,10 +20,33 @@
   },
   "NTProvider": {
     "types": {
-      "/FMSInfo": "FMSInfo"
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Task Selector": "String Chooser"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "Task Selector": {
+          "open": true,
+          "string[]##v_/SmartDashboard/Task Selector/options": {
+            "open": true
+          }
+        },
+        "open": true
+      }
     }
   },
   "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Connections": {
+      "open": true
+    },
+    "Server": {
+      "open": true
+    },
     "visible": true
   }
 }

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

@@ -4,9 +4,21 @@
 // Credit to Team 5338 for any changes to the base command XRP template.
 package frc.robot;
 
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.DriveCommands;
+import frc.robot.subsystems.Servo;
+import frc.robot.subsystems.XRPDrivetrain;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -18,6 +30,7 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  private SendableChooser<Integer> m_chooser = new SendableChooser<>();
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -28,6 +41,13 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    for(int i=1; i<=10; i++){
+      m_chooser.addOption("Task " + i, i);
+    }
+    m_chooser.addOption("Task 3 Bonus", 11);
+    m_chooser.addOption("Task 7 Bonus", 12);
+    SmartDashboard.putData("Task Selector", m_chooser);
+    SmartDashboard.putString("What should happen: ", "");
   }
 
   /**
@@ -61,7 +81,122 @@ public void autonomousInit() {
     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!)");
+        DriveCommands.turnDegreesCommand(1, 90).schedule();
+        break;
+      case 2:
+      //TODO for Kavin: Replace the instant commands with runOnce
+        SmartDashboard.putString("What should happen: ", "The XRP should drive straight for 3 seconds, turn right for 2 seconds, turn left for 2 seconds, and then stop.");
+        new SequentialCommandGroup(
+          new ParallelDeadlineGroup(
+            new WaitCommand(3), 
+            new InstantCommand(() -> {
+              try {
+                  XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 1, 1);
+                  System.out.println("runs");
+              } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) {
+                  // TODO Auto-generated catch block
+                  e.printStackTrace();
+              }
+              }, 
+              RobotContainer.m_xrpDrivetrain
+            )
+          ),
+          new ParallelDeadlineGroup(
+            new WaitCommand(2), 
+            new InstantCommand(() -> {
+              try {
+                  XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 1, -1);
+                  System.out.println("runs");
+              } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) {
+                  // TODO Auto-generated catch block
+                  e.printStackTrace();
+              }
+              }, 
+              RobotContainer.m_xrpDrivetrain
+            )
+          ),
+          new ParallelDeadlineGroup(
+            new WaitCommand(2), 
+            new InstantCommand(() -> {
+              try {
+                  XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, -1, 1);
+                  System.out.println("runs");
+              } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) {
+                  // TODO Auto-generated catch block
+                  e.printStackTrace();
+              }
+              }, 
+              RobotContainer.m_xrpDrivetrain
+            )
+          ),
+          new InstantCommand(() -> {
+            try {
+                XRPDrivetrain.class.getMethod("arcadeDrive", double.class).invoke(RobotContainer.m_xrpDrivetrain, 0, 0);
+                System.out.println("runs");
+            } catch (NoSuchMethodException | IllegalAccessException | InvocationTargetException e) {
+                  // TODO Auto-generated catch block
+                  e.printStackTrace();
+              }
+            }, 
+            RobotContainer.m_xrpDrivetrain
+        )).schedule();
+        break;
+      case 3:
+        SmartDashboard.putString("What should happen: ", "The XRP should drive 5 inches(don't worry if it's not accurate!). tankDriveCommand will be tested in Task 4");
+        try {
+          DriveCommands.class.getMethod("tankDriveDistance", double.class).invoke(null, 5);
+        } catch (NoSuchMethodException e) {
+          // TODO Auto-generated catch block
+          try {
+            ((Command) Class.forName("DriveCommands$TankDriveDistance").getDeclaredConstructor(double.class).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 4:
+        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 5:
+        SmartDashboard.putString("What should happen: ", "The arm should move to 30 degrees, move to 142.5 degrees, and put the number of degrees on SmartDashboard(should be 142.5)");
+        try {
+          Servo.class.getMethod("setServoAngle", double.class).invoke(RobotContainer.m_servo, 30);
+          Servo.class.getMethod("setServoAngle", double.class).invoke(RobotContainer.m_servo, 142.5);
+          SmartDashboard.putNumber("Servo Angle", (Double) Servo.class.getMethod("getServoAngle").invoke(RobotContainer.m_servo));
+        } catch (IllegalAccessException | IllegalArgumentException | InvocationTargetException | NoSuchMethodException
+            | SecurityException e) {
+          // TODO Auto-generated catch block
+          e.printStackTrace();
+        }
+        break;
+      case 6:
+        SmartDashboard.putString("What should happen: ", "The XRP should move its servo to each of the presets.");
+        try{
+
+        }
+        break;
+      case 7:
+        break;
+        case 8:
+        break;
+      case 9:
+        break;
+      case 10:
+        break;
+      case 11:
+        break;
+      case 12:
+        break;
+    }
   }
+
 
   /** This function is called periodically during autonomous. */
   @Override

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

@@ -197,13 +197,22 @@ 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
+  //TODO: Task 1-Rewrite TurnDegrees as a function that returns a functional command by replacing each null value with the correct code
   //Code here:
-
+  public static Command turnDegreesCommand(double speed, double degrees){
+    return new FunctionalCommand(
+      null, /* This is what runs when the command initializes. */
+      null, /* This happens periodically when the command is executed*/
+      null, /* This is what runs when the command is complete */
+      null, /* This is what determines if the command is complete */
+      null /* This is where you write any and all subsystems used in the command. The drivetrain is RobotContainer.m_xrpDrivetrain. Perhaps you can find
+                            a way to avoid having to type out the entire name of the drivetrain every time you reference it?*/
+    );
+  }
   //TODO: Task 3-Rewrite the following functions to use tank drive
-  //driveDistance or AltDriveDistance(choose 1)
-  //arcadeDriveCommand(rename as tankDriveCommand)
-  //Bonus(optional):TurnDegrees(either class or function, choose 1)
+  //driveDistance or AltDriveDistance(choose 1) (name as tankDriveDistance or TankDriveDistance based on if you chose to rewrite the former or the latter)
+  //arcadeDriveCommand(name as tankDriveCommand)
+  //Bonus(optional):TurnDegrees(either class or function, choose 1) (name as tankTurnDegrees or TankTurnDegrees based on if you chose to rewrite the former or the latter)
 
   /**
    * Commands can also a part of command groups, which are sets of commands that run in certain ways. Sequential Command Groups

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

@@ -3,6 +3,6 @@
 
 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.
+    //Name them servoUpCommand, servoDownCommand, and servoPresetCommand. The presets are in the constants file.
 
 }

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

@@ -11,7 +11,7 @@ public Servo(){
         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
-    //it is at.
+    //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
     //HINT: Type 'm_armServo.' (without quotes) in a method body to see all of the different methods the servo has.
 }

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

@@ -45,8 +45,9 @@ public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
     m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
   }
 
-  //TODO:Task 2-Write a function that makes the robot take in a left wheel speed and a right wheel speed, and move at those speeds.
-  //HINT:This is called a tank drive, maybe there is a function that can easily allow for it?
+  //TODO:Task 2-Write a method that makes the robot take in a left wheel speed and a right wheel speed, and move at those speeds.
+  //This type of robot control is called tank drive, so name the function tankDrive (exactly as written)
+  //This should be a method that does not return anything, not a command!
   //HINT:To see all the functions that the drivetrain has, type "m_diffDrive." without the quotes and look at the options generated by auto complete.
 
   public void resetEncoders() {