Task 3 Completion

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc.robot.Main",
+      "projectName": "XRP-Template"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

src/main/java/frc/robot/Main.java

@@ -12,7 +12,8 @@
  * call.
  */
 public final class Main {
-  private Main() {}
+  private Main() {
+  }
 
   /**
    * Main initialization function. Do not perform any initialization here.

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

@@ -199,10 +199,28 @@ 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 m_degrees, double m_speed){
+    XRPDrivetrain m_drive = RobotContainer.m_xrpDrivetrain;
+    return new FunctionalCommand(()->{m_drive.arcadeDrive(0, 0);m_drive.resetEncoders();},
+    ()->{m_drive.arcadeDrive(0, m_speed);}, interrupt -> {m_drive.arcadeDrive(0, 0);}, 
+    () -> {double inchPerDegree = Math.PI * 6.102 / 360; return m_drive.getAverageTurningDistance() >= (inchPerDegree * m_degrees);} , m_drive
+    );
+  }
 
   //TODO: Task 3-Rewrite the following functions to use tank drive
   //driveDistance or AltDriveDistance(choose 1)
   //arcadeDriveCommand(rename as tankDriveCommand)
+
+public static Command tankDriveCommand(Supplier<Double> forwardSpeed, Supplier<Double> turnSpeed){
+    //This uses an InstantCommand, which shouldn't be a class. An Instant Command immediately executes, and only takes in fields for what it should do
+    //and the required subsystems.
+    //Useful for simple commands.
+    return new InstantCommand(
+      //Tells the XRP to drive at the given speeds by using .get(), which gets the value returned by the supplier
+      ()->RobotContainer.m_xrpDrivetrain.tankDrive(),//(forwardSpeed.get(), turnSpeed.get()), 
+      RobotContainer.m_xrpDrivetrain
+      );
+  }
   //Bonus(optional):TurnDegrees(either class or function, choose 1)
 
   /**

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

@@ -45,10 +45,16 @@ 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?
   //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 tankDrive(){
+    m_diffDrive.tankDrive(kCountsPerRevolution, kCountsPerMotorShaftRev);
+  }
+
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();