From 6346126e28dcff2de034907b7aa7c81bd2c74102 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Wed, 8 Feb 2023 00:35:10 -0700
Subject: [PATCH] basic joystick recording and playback test

---
 .../frc4388/robot/commands/JoystickInputs.txt |  0
 .../robot/commands/JoystickPlayback.java      | 73 +++++++++++++++++++
 .../robot/commands/JoystickRecorder.java      | 59 +++++++++++++++
 3 files changed, 132 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/JoystickInputs.txt
 create mode 100644 src/main/java/frc4388/robot/commands/JoystickPlayback.java
 create mode 100644 src/main/java/frc4388/robot/commands/JoystickRecorder.java

diff --git a/src/main/java/frc4388/robot/commands/JoystickInputs.txt b/src/main/java/frc4388/robot/commands/JoystickInputs.txt
new file mode 100644
index 0000000..e69de29
diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
new file mode 100644
index 0000000..d26a818
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.Scanner;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.controller.DeadbandedXboxController;
+
+public class JoystickPlayback extends CommandBase {
+
+  SwerveDrive swerve;
+  Scanner input;
+
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.swerve = swerve;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    try {
+      input = new Scanner(new File("JoystickInput.txt"));
+    } catch (FileNotFoundException e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    String line = "";
+    if (input.hasNextLine()) {
+      line = input.nextLine();
+    }
+
+    int ileftX = line.indexOf("leftX: ");
+    int ileftY = line.indexOf(", leftY: ");
+    int irightX = line.indexOf(", rightX: ");
+    int irightY = line.indexOf(", rightY: ");
+
+    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));
+
+    this.swerve.driveWithInput(new Translation2d(leftX, leftY), new Translation2d(-rightX, rightY), true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    input.close();
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
new file mode 100644
index 0000000..d69eb6e
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.controller.DeadbandedXboxController;
+
+public class JoystickRecorder extends CommandBase {
+
+  DeadbandedXboxController joystick;
+  Supplier<DeadbandedXboxController> joystickSupplier;
+
+  int numEntries = 0;
+
+  /** Creates a new JoystickRecorder. */
+  public JoystickRecorder(Supplier<DeadbandedXboxController> joystickSupplier) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.joystickSupplier = joystickSupplier;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    joystick = joystickSupplier.get();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double leftX = joystick.getLeftX();
+    double leftY = joystick.getLeftY();
+    double rightX = joystick.getRightX();
+    double rightY = joystick.getRightY();
+
+    try {
+      Files.writeString(Path.of("JoystickInputs.txt"), "leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY);
+      numEntries = numEntries + 1;
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return numEntries > 10;
+  }
+}