commit (incomplete)

src/main/java/frc4388/robot/Robot.java

@@ -10,6 +10,7 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.robot.commands.JoystickRecorder;
 import frc4388.utility.RobotTime;
 
 /**
@@ -25,6 +26,8 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private JoystickRecorder m_joystickRecorder;
+  Runnable recorder;
 
   /**
    * This function is run when the robot is first started up and should be
@@ -93,6 +96,9 @@ public class Robot extends TimedRobot {
 
   @Override
   public void teleopInit() {
+
+    System.out.println("TELEOP INIT STARTING");
+
     m_robotContainer.m_robotSwerveDrive.resetGyro();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
@@ -103,14 +109,30 @@ public class Robot extends TimedRobot {
     }
     m_robotContainer.m_robotSwerveDrive.resetGyro();
     m_robotTime.startMatchTime();
+
+    m_joystickRecorder = new JoystickRecorder(m_robotContainer.m_robotSwerveDrive,
+                                              () -> m_robotContainer.getDeadbandedDriverController().getLeftX(),
+                                              () -> m_robotContainer.getDeadbandedDriverController().getLeftY(),
+                                              () -> m_robotContainer.getDeadbandedDriverController().getRightX(),
+                                              () -> m_robotContainer.getDeadbandedDriverController().getRightY()
+                                            );
+
+    m_joystickRecorder.initialize();
+
+    // recorder = () -> {
+    //   m_joystickRecorder.execute();
+    // };
+
+    addPeriodic(() -> {
+      m_joystickRecorder.execute();
+    }, 0.1);
   }
 
   /**
    * This function is called periodically during operator control.
    */
   @Override
-  public void teleopPeriodic() {
+  public void teleopPeriodic() {}
-  }
 
   /**
    * This function is called periodically during test mode.

src/main/java/frc4388/robot/commands/JoystickRecorder.java

@@ -45,6 +45,8 @@ public class JoystickRecorder extends CommandBase {
     this.startTime = System.currentTimeMillis();
 
     outputs.add(new TimedOutput());
+
+    System.out.println("STARTING RECORDING");
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -62,11 +64,15 @@ public class JoystickRecorder extends CommandBase {
     swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
                           new Translation2d(inputs.rightX, inputs.rightY),
                           true);
+    
+    System.out.println("RECORDING IN PROGRESS");
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    System.out.println("RECORDING ENDED");
+
     File output = new File("/home/lvuser/JoystickInputs.txt");
 
     try (PrintWriter writer = new PrintWriter(output)) {
@@ -85,6 +91,6 @@ public class JoystickRecorder extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return ((System.currentTimeMillis() - this.startTime) > 15000);
   }
 }