AUTO RECORDING WORKS

2026-06-09 08:38:02 -06:00 · 2023-02-10 20:53:33 -07:00
parent 14417df283
commit 3e85815b2d
4 changed files with 194 additions and 18 deletions
@@ -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)
@@ -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
@@ -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.
@@ -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<Double> rightXSupplier;
  Supplier<Double> rightYSupplier;
  
-  HashMap<Long, double[]> timedInput;
+  // HashMap<Long, double[]> timedInput;
+  ArrayList<double[]> 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<Long, double[]>();
+    this.outputs = new ArrayList<double[]>();
+
+    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.