From 50e407eb9f22e7e23d6f86de759c0b139ecd163b Mon Sep 17 00:00:00 2001
From: Ishan <98932677+Ishan1522@users.noreply.github.com>
Date: Tue, 7 Jan 2025 15:08:48 -0500
Subject: [PATCH] meh

---
 .../extras/swerve/RepulsorFieldPlanner.java   |  1 +
 .../frc/robot/extras/util/AlertsUtil.java     |  2 +-
 .../robot/subsystems/swerve/SwerveDrive.java  | 43 +++++++++++++++++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/extras/swerve/RepulsorFieldPlanner.java b/src/main/java/frc/robot/extras/swerve/RepulsorFieldPlanner.java
index d712133..680e4a3 100644
--- a/src/main/java/frc/robot/extras/swerve/RepulsorFieldPlanner.java
+++ b/src/main/java/frc/robot/extras/swerve/RepulsorFieldPlanner.java
@@ -21,6 +21,7 @@
 import edu.wpi.first.networktables.NetworkTableEvent.Kind;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.extras.util.Force;
 
 @Logged
 public class RepulsorFieldPlanner {
diff --git a/src/main/java/frc/robot/extras/util/AlertsUtil.java b/src/main/java/frc/robot/extras/util/AlertsUtil.java
index ead0c80..b13cfc8 100644
--- a/src/main/java/frc/robot/extras/util/AlertsUtil.java
+++ b/src/main/java/frc/robot/extras/util/AlertsUtil.java
@@ -1,4 +1,4 @@
-package frc.robot.util;
+package frc.robot.extras.util;
 
 import java.util.function.BooleanSupplier;
 
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
index ef30158..ac7e310 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -13,8 +13,10 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
+import frc.robot.extras.swerve.RepulsorFieldPlanner;
 import frc.robot.extras.swerve.setpointGen.SwerveSetpoint;
 import frc.robot.extras.swerve.setpointGen.SwerveSetpointGenerator;
 import frc.robot.extras.util.DeviceCANBus;
@@ -28,9 +30,13 @@
 import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged;
 import frc.robot.subsystems.vision.VisionConstants;
 import java.util.Optional;
+import java.util.function.Supplier;
+
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+import choreo.trajectory.SwerveSample;
+
 public class SwerveDrive extends SubsystemBase {
   private final GyroInterface gyroIO;
   private final GyroInputsAutoLogged gyroInputs;
@@ -58,6 +64,8 @@ public class SwerveDrive extends SubsystemBase {
 
   private Optional<DriverStation.Alliance> alliance;
 
+  private RepulsorFieldPlanner repulsor = new RepulsorFieldPlanner();
+
   private final Alert gyroDisconnectedAlert =
       new Alert("Gyro Hardware Fault", Alert.AlertType.kError);
 
@@ -203,6 +211,41 @@ xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
     Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates());
   }
 
+  /*
+   * @return The current ChassisSpeeds of the robot.
+   */
+  public ChassisSpeeds getChassisSpeeds() {
+    return setpoint.chassisSpeeds();
+  }
+
+    // public Command repulsorCommand(Supplier<Pose2d> target) {
+    //     return run(()->{
+    //         repulsor.setGoal(target.get().getTranslation());
+    //         followPath(getPose(), repulsor.getCmd(getPose(), getChassisSpeeds(), 4, true, target.get().getRotation()));
+    //     });
+    // }
+
+    // public void followPath(Pose2d pose, SwerveSample sample) {
+    //     m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    //     var targetSpeeds = sample.getChassisSpeeds();
+    //     targetSpeeds.vxMetersPerSecond += m_pathXController.calculate(
+    //         pose.getX(), sample.x
+    //     );
+    //     targetSpeeds.vyMetersPerSecond += m_pathYController.calculate(
+    //         pose.getY(), sample.y
+    //     );
+    //     targetSpeeds.omegaRadiansPerSecond += m_pathThetaController.calculate(
+    //         pose.getRotation().getRadians(), sample.heading
+    //     );
+
+    //     setControl(
+    //         m_pathApplyFieldSpeeds.withSpeeds(targetSpeeds)
+    //             .withWheelForceFeedforwardsX(sample.moduleForcesX())
+    //             .withWheelForceFeedforwardsY(sample.moduleForcesY())
+    //     );
+    // }
+
   /**
    * Returns the heading of the robot in degrees from 0 to 360.
    *