From b5a9f59c59df3a4473ffb01dabad5f52a495ba1d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 10 Feb 2023 20:53:33 -0700 Subject: [PATCH] AUTO RECORDING WORKS --- .../java/frc4388/robot/RobotContainer.java | 19 ++- .../frc4388/robot/commands/JoystickInputs.txt | 132 ++++++++++++++++++ .../robot/commands/JoystickPlayback.java | 24 ++-- .../robot/commands/JoystickRecorder.java | 37 +++-- 4 files changed, 194 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a3690d9..aa94317 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,8 @@ package frc4388.robot; +import javax.print.attribute.standard.OrientationRequested; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -14,6 +16,8 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; +import frc4388.robot.commands.JoystickPlayback; +import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.IHandController; @@ -76,7 +80,20 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + () -> getDeadbandedDriverController().getLeftX(), + () -> getDeadbandedDriverController().getLeftY(), + () -> getDeadbandedDriverController().getRightX(), + () -> getDeadbandedDriverController().getRightY() + + )) + .onFalse(new InstantCommand()); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive)); + // /* Operator Buttons */ // // interrupt button // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt index e69de29..8ca32ac 100644 --- a/src/main/java/frc4388/robot/commands/JoystickInputs.txt +++ b/src/main/java/frc4388/robot/commands/JoystickInputs.txt @@ -0,0 +1,132 @@ +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,-0.1015625,0.0,0.0 +0.0,-0.109375,0.0,0.0 +0.0,-0.1171875,0.0,0.0 +0.0,-0.140625,0.0,0.0 +0.0,-0.140625,0.0,0.0 +0.0,-0.1484375,0.0,0.0 +0.0,-0.1796875,0.0,0.0 +0.0,-0.1796875,0.0,0.0 +0.0,-0.1875,0.0,0.0 +0.0,-0.1953125,0.0,0.0 +0.0,-0.21875,0.0,0.0 +0.0,-0.2265625,0.0,0.0 +0.0,-0.25,0.0,0.0 +0.0,-0.2890625,0.0,0.0 +0.0,-0.3046875,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3125,0.0,0.0 +0.0,-0.3359375,0.0,0.0 +0.0,-0.359375,0.0,0.0 +0.0,-0.390625,0.0,0.0 +0.0,-0.3984375,0.0,0.0 +0.0,-0.40625,0.0,0.0 +0.0,-0.4296875,0.0,0.0 +0.0,-0.4453125,0.0,0.0 +0.0,-0.4609375,0.0,0.0 +0.0,-0.46875,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.5,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.4921875,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.484375,0.0,0.0 +0.0,-0.46875,0.0,0.0 +0.0,-0.4296875,0.0,0.0 +0.0,-0.359375,0.0,0.0 +0.0,-0.2734375,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 +0.0,0.0,0.0,0.0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java index 774cffa..a15eb24 100644 --- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java @@ -26,16 +26,23 @@ public class JoystickPlayback extends CommandBase { public JoystickPlayback(SwerveDrive swerve) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; + + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { try { - input = new Scanner(new File("JoystickInput.txt")); + input = new Scanner(new File("/home/lvuser/JoystickInputs.txt")); } catch (FileNotFoundException e) { e.printStackTrace(); } + + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); + System.out.println("STARTING PLAYBACK"); } // Called every time the scheduler runs while the command is scheduled. @@ -47,17 +54,16 @@ public class JoystickPlayback extends CommandBase { line = input.nextLine(); } - int ileftX = line.indexOf("leftX: "); - int ileftY = line.indexOf(", leftY: "); - int irightX = line.indexOf(", rightX: "); - int irightY = line.indexOf(", rightY: "); + String[] values = line.split(","); - double leftX = Double.parseDouble(line.substring(ileftX + 1, ileftY)); - double leftY = Double.parseDouble(line.substring(ileftY + 1, irightX)); - double rightX = Double.parseDouble(line.substring(irightX + 1, irightY)); - double rightY = Double.parseDouble(line.substring(irightY + 1)); + double leftX = Double.parseDouble(values[0]); + double leftY = Double.parseDouble(values[1]); + double rightX = Double.parseDouble(values[2]); + double rightY = Double.parseDouble(values[3]); this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true); + + System.out.println("PLAYING"); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java index ea14c81..134af83 100644 --- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java @@ -9,6 +9,7 @@ import java.io.IOException; import java.io.PrintWriter; import java.nio.file.Files; import java.nio.file.Path; +import java.util.ArrayList; import java.util.HashMap; import java.util.function.Supplier; @@ -27,9 +28,10 @@ public class JoystickRecorder extends CommandBase { Supplier rightXSupplier; Supplier rightYSupplier; - HashMap timedInput; + // HashMap timedInput; + ArrayList outputs; - private long startTime; + private final long startTime; /** Creates a new JoystickRecorder. */ @@ -40,38 +42,57 @@ public class JoystickRecorder extends CommandBase { this.leftYSupplier = leftYSupplier; this.rightXSupplier = rightXSupplier; this.rightYSupplier = rightYSupplier; + + this.startTime = System.currentTimeMillis(); + // this.timedInput = new HashMap(); + this.outputs = new ArrayList(); + + addRequirements(this.swerve); } // Called when the command is initially scheduled. @Override public void initialize() { - timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); - startTime = System.currentTimeMillis(); + // timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0}); + outputs.add(new double[] {0.0, 0.0, 0.0, 0.0}); + + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); + System.out.println("STARTING RECORDING"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { double[] inputs = new double[] {leftXSupplier.get(), leftYSupplier.get(), rightXSupplier.get(), rightYSupplier.get()}; - timedInput.put(System.currentTimeMillis() - startTime, inputs); + // timedInput.put(System.currentTimeMillis() - startTime, inputs); + outputs.add(inputs); swerve.driveWithInput(new Translation2d(inputs[0], inputs[1]), new Translation2d(-inputs[2], inputs[3]), true); + + System.out.println("RECORDING"); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - File output = new File("JoystickInputs.txt"); + File output = new File("/home/lvuser/JoystickInputs.txt"); try(PrintWriter writer = new PrintWriter(output)) { - for(long millis : timedInput.keySet()) { - writer.println("time: " + millis + ", leftX: " + timedInput.get(millis)[0] + ", leftY: " + timedInput.get(millis)[1] + ", rightX: " + timedInput.get(millis)[2] + ", rightY: " + timedInput.get(millis)[3]); + for(double[] input : outputs) { + writer.println(input[0] + "," + input[1] + "," + input[2] + "," + input[3]); } writer.close(); } catch(IOException e) { e.printStackTrace(); } + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + System.out.println("STOPPED RECORDING"); + } // Returns true when the command should end.