From f9b237592f4b724c4ccb7fa5a03a91188a05dd87 Mon Sep 17 00:00:00 2001
From: Abhi <90010729+Abhishrek05@users.noreply.github.com>
Date: Fri, 10 Feb 2023 18:12:56 -0700
Subject: [PATCH] untested joystick recording

---
 .../robot/commands/JoystickRecorder.java      | 58 +++++++++++++------
 1 file changed, 40 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
index d658a6d..ea14c81 100644
--- a/src/main/java/frc4388/robot/commands/JoystickRecorder.java
+++ b/src/main/java/frc4388/robot/commands/JoystickRecorder.java
@@ -4,53 +4,75 @@
 
 package frc4388.robot.commands;
 
+import java.io.File;
 import java.io.IOException;
+import java.io.PrintWriter;
 import java.nio.file.Files;
 import java.nio.file.Path;
+import java.util.HashMap;
 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.RobotTime;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 public class JoystickRecorder extends CommandBase {
-
-  DeadbandedXboxController joystick;
-  Supplier<DeadbandedXboxController> joystickSupplier;
   
-  RobotTime time;
+  SwerveDrive swerve;
+
+  Supplier<Double> leftXSupplier;
+  Supplier<Double> leftYSupplier;
+  Supplier<Double> rightXSupplier;
+  Supplier<Double> rightYSupplier;
+  
+  HashMap<Long, double[]> timedInput;
+
+  private long startTime;
+
 
   /** Creates a new JoystickRecorder. */
-  public JoystickRecorder(Supplier<DeadbandedXboxController> joystickSupplier) {
+  public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftXSupplier, Supplier<Double> leftYSupplier, Supplier<Double> rightXSupplier, Supplier<Double> rightYSupplier) {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.joystickSupplier = joystickSupplier;
-    time = RobotTime.getInstance();
+    this.swerve = swerve;
+    this.leftXSupplier = leftXSupplier;
+    this.leftYSupplier = leftYSupplier;
+    this.rightXSupplier = rightXSupplier;
+    this.rightYSupplier = rightYSupplier;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    joystick = joystickSupplier.get();
+    timedInput.put((long) 0, new double[] {0.0, 0.0, 0.0, 0.0});
+    startTime = System.currentTimeMillis();
   }
 
   // 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();
+    double[] inputs = new double[] {leftXSupplier.get(), leftYSupplier.get(), rightXSupplier.get(), rightYSupplier.get()};
+    timedInput.put(System.currentTimeMillis() - startTime, inputs);
 
-    try {
-      Files.writeString(Path.of("JoystickInputs.txt"), "Time: " + time.m_robotTime + ", leftX: " + leftX + ", " + "leftY: " + leftY + ", " + "rightX: " + rightX + ", " + "rightY: " + rightY + "\n");
-    } catch (IOException e) {
-      e.printStackTrace();
-    }
+    swerve.driveWithInput(new Translation2d(inputs[0], inputs[1]), new Translation2d(-inputs[2], inputs[3]), true);
   }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    File output = new File("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]);
+      }
+      writer.close();
+    } catch(IOException e) {
+        e.printStackTrace();
+    }
+
+  }
 
   // Returns true when the command should end.
   @Override