diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2d6f348..79d9ade 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index 2b3fd24..f3b1f53 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/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); } }