From 9a3f66f877f8f51f9571c1ed5fe8e5e31982255e Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Sun, 31 Mar 2024 21:54:36 -0400
Subject: [PATCH] added command that can be used to get new lerp points and
 print the code to use for those lerp points

---
 .../frc/robot/commands/auto/AutonShoot.java   |  2 -
 .../robot/commands/auto/SaveLerpPoints.java   | 77 +++++++++++++++++++
 2 files changed, 77 insertions(+), 2 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/auto/SaveLerpPoints.java

diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java
index bba41d6..005cdaf 100644
--- a/src/main/java/frc/robot/commands/auto/AutonShoot.java
+++ b/src/main/java/frc/robot/commands/auto/AutonShoot.java
@@ -7,13 +7,11 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.commands.intake.FeedActuate;
 import frc.robot.commands.shooter.SpinShooter;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.FeedMode;
diff --git a/src/main/java/frc/robot/commands/auto/SaveLerpPoints.java b/src/main/java/frc/robot/commands/auto/SaveLerpPoints.java
new file mode 100644
index 0000000..26c946d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/SaveLerpPoints.java
@@ -0,0 +1,77 @@
+// 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 frc.robot.commands.auto;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.utils.UtilFuncs;
+
+public class SaveLerpPoints extends Command {
+  private final ShooterSubsystem _shooter;
+  private final ElevatorSubsystem _elevator;
+
+  private ArrayList<double[]> shooterPoints = new ArrayList<>();
+  private ArrayList<double[]> elevatorPoints = new ArrayList<>();
+
+  private final String SHOOTER_LERP_NAME = "SHOOTER_DISTANCE_ANGLE";
+  private final String ELEVATOR_LERP_NAME = "ELEVATOR_DISTANCE_HEIGHT";
+
+  /** Creates a new SaveLerpPoints. */
+  public SaveLerpPoints(
+    ShooterSubsystem shooter,
+    ElevatorSubsystem elevator
+  ) {
+    _shooter = shooter;
+    _elevator = elevator;
+
+    addRequirements(_shooter, _elevator);
+  }
+
+  private void showLine(String className, double[] point) {
+    System.out.println(className + ".put(" + Double.toString(point[0]) + ", " + Double.toString(point[1]) + ");");
+  }
+
+  /**
+   * Creates the code that can be copied into Constants.java for lerp points.
+   */
+  public void showCode() {
+    for (double[] point : shooterPoints) {
+      showLine(SHOOTER_LERP_NAME, point);
+    }
+
+    for (double[] point : elevatorPoints) {
+      showLine(ELEVATOR_LERP_NAME, point);
+    }
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    double shotDistance = UtilFuncs.ShotVector().getNorm();
+
+    double[] shooterLerp = {shotDistance, _shooter.getAngle()};
+    double[] elevatorLerp = {shotDistance, _elevator.getHeight()};
+
+    shooterPoints.add(shooterLerp);
+    elevatorPoints.add(elevatorLerp);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // 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 true;
+  }
+}