From 5a1ff5c8b450f03d6e32ae4adac1a37ba6478a35 Mon Sep 17 00:00:00 2001
From: cherriae <zeruiz@nycstudents.net>
Date: Wed, 20 Mar 2024 18:29:23 -0400
Subject: [PATCH] Revert "NeuralNetwork support"

This reverts commit c69dffae4c77525aa00350ef27330cd520ad6dcc.
---
 .../frc/robot/commands/auto/NoteAllign.java   | 75 -------------------
 .../frc/robot/subsystems/VisionSubsystem.java |  9 ---
 .../robot/utils/helpers/LimelightHelper.java  | 32 +++-----
 3 files changed, 9 insertions(+), 107 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/auto/NoteAllign.java

diff --git a/src/main/java/frc/robot/commands/auto/NoteAllign.java b/src/main/java/frc/robot/commands/auto/NoteAllign.java
deleted file mode 100644
index 157a742..0000000
--- a/src/main/java/frc/robot/commands/auto/NoteAllign.java
+++ /dev/null
@@ -1,75 +0,0 @@
-// 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.function.DoubleSupplier;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.Constants.PID;
-import frc.robot.Constants.Speeds;
-import frc.robot.subsystems.SwerveDriveSubsystem;
-import frc.robot.subsystems.VisionSubsystem;
-
-public class NoteAllign extends Command {
-
-  private SwerveDriveSubsystem _swerve;
-  private VisionSubsystem _vision;
-
-  private DoubleSupplier _xSpeed;
-  private DoubleSupplier _ySpeed;
-
-
-  private PIDController _headingController = new PIDController(0, 0, 0);
-
-  /** Creates a new NoteAllign. */
-  public NoteAllign(SwerveDriveSubsystem swerve, VisionSubsystem vision, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    _swerve = swerve;
-    _vision = vision;
-
-    _xSpeed = xSpeed;
-    _ySpeed = ySpeed;
-
-    addRequirements(_swerve, _vision);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    double noteX = _vision.getNoteAngles()[0];
-    
-    double rotationVelocity = MathUtil.clamp(
-      -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
-      Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,
-      _headingController.calculate(noteX, 0)
-    );
-
-    _swerve.driveChassis(new ChassisSpeeds(
-      _xSpeed.getAsDouble(),
-      _ySpeed.getAsDouble(),
-      rotationVelocity 
-    ));
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    _swerve.driveChassis(new ChassisSpeeds());
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return _headingController.atSetpoint();
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
index acefaf0..8188b44 100644
--- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -27,8 +27,6 @@
  */
 public class VisionSubsystem extends SubsystemBase {
   private final LimelightHelper _main = new LimelightHelper("main");
-  private final LimelightHelper _intake = new LimelightHelper("intake");
-
 
   private final MedianFilter _xFilter = new MedianFilter(20); // TODO: change?
   private final MedianFilter _yFilter = new MedianFilter(20);
@@ -176,11 +174,4 @@ public double[] tagAngleOffsets(int ID) {
 
     return angles;
   }
-
-  public double[] getNoteAngles() {
-    JsonNode target = _intake.getNeuralTarget();
-    double[] angles = {target.get("tx").asDouble(), target.get("ty").asDouble()};
-
-    return angles;
-  }
 }
diff --git a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
index 14f4343..f2f9a33 100644
--- a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
+++ b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
@@ -3,8 +3,6 @@
 
 import com.fasterxml.jackson.databind.JsonNode;
 import com.fasterxml.jackson.databind.ObjectMapper;
-import com.fasterxml.jackson.databind.node.ArrayNode;
-
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -30,33 +28,21 @@ public NetworkTableEntry getEntry(String name) {
     return _limelight.getEntry(name);
   }
 
-  /** Return json from limelight. */
-  public JsonNode getJson() {
-    String jsonString = getEntry("json").getString("");
-    try {
-      return _objectMapper.readTree(jsonString);
-    } catch (Exception e) {
-      throw new Error("Cannot Read JSON From Limelight.");
-    }
-  }
-
-  /** 
-   * Returns the neural target from the limelight.
-   * 
-   */
-  public JsonNode getNeuralTarget() {
-    ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector");
-
-    return targets.get(0);
-  }
-
   /**
    * Returns a JsonNode array containing found tags and their info.
    *
    * @see JsonNode
    */
   public JsonNode getTags() {
-    JsonNode tags = getJson().get("Results").get("Fiducial");
+    String jsonString = getEntry("json").getString("");
+
+    JsonNode tags;
+
+    try {
+      tags = _objectMapper.readTree(jsonString).get("Results").get("Fiducial");
+    } catch (Exception e) {
+      throw new Error("Cannot Read JSON From Limelight.");
+    }
 
     return tags;
   }