From a8a5cf6d514fb699dd8a5c8e9b8f235949461f4b Mon Sep 17 00:00:00 2001
From: WispySparks <wispysparks@gmail.com>
Date: Tue, 10 Sep 2024 17:40:51 -0500
Subject: [PATCH] joystick control start

---
 src/main/java/frc/robot/Constants.java        |  4 ++
 src/main/java/frc/robot/Robot.java            |  8 +++
 .../frc/robot/commands/AbsoluteDriveAdv.java  | 70 +++++++++++++++++++
 .../java/frc/robot/subsystems/Swerve.java     | 10 +++
 4 files changed, 92 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/AbsoluteDriveAdv.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 027dfd6..26049f7 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -32,4 +32,8 @@ public static class SwerveK {
 
         public static final File swerveDirectory = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/swerve");
     }
+
+    public static class ControllerK {
+        public static final int xboxPort = 0; //! Find
+    }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 08ba675..4390dc5 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,13 +5,21 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.ControllerK;
 import frc.robot.subsystems.Swerve;
 
 public class Robot extends TimedRobot {
 
     private final Swerve swerve = new Swerve();
+
+    final CommandXboxController xboxController = new CommandXboxController(ControllerK.xboxPort);
     
     public Robot() {
+        configureController();
+    }
+
+    private void configureController() {
         
     }
 
diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java
new file mode 100644
index 0000000..9fc77cc
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java
@@ -0,0 +1,70 @@
+package frc.robot.commands;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import frc.robot.subsystems.Swerve;
+
+public class AbsoluteDriveAdv {
+
+    private final Swerve swerve; 
+    private final DoubleSupplier vX, vY, headingAdjust; 
+    private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
+    private boolean resetHeading;
+
+    /**
+     * @param swerve Swerve Subsystem
+     * @param vX Double supplier X component of joystick input after deadband is applied, should be -1 to 1
+     * @param vY Double supplier Y component of joystick input after deadband is applied, should be -1 to 1
+     * @param headingAdjust Double supplier current robot heading to be adjusted after deadband is applied
+     * @param lookAway Faces the robot towards the opposind alliance wall from the driver
+     * @param lookTowards Faces the robot towards the driver
+     * @param lookLeft Faces the robot left
+     * @param lookRight Faces the robot right
+     * 
+     * @return
+     */ 
+    public AbsoluteDriveAdv(Swerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust,
+                          BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft,
+                          BooleanSupplier lookRight) {
+
+                            this.swerve = swerve;
+                            this.vX = vX;
+                            this.vY = vY;
+                            this.headingAdjust = headingAdjust;
+                            this.lookAway = lookAway;
+                            this.lookTowards = lookTowards;
+                            this.lookLeft = lookLeft;
+                            this.lookRight = lookRight;
+    }
+
+    public void initialize() {
+        resetHeading = true;
+    }
+
+    public void execute() {
+        double headingX = 0; // Resets values
+        double headingY = 0;
+
+        if (lookAway.getAsBoolean()) { // Y
+            headingY = -1;
+        }
+        if (lookTowards.getAsBoolean()) { // Y
+            headingY = 1;
+        }
+        if (lookLeft.getAsBoolean()) { // X
+            headingX = -1;
+        }
+        if (lookRight.getAsBoolean()) { // X
+            headingX = 1;
+        }
+    }
+
+    public void end() {
+
+    }
+
+    public boolean isFinished() {
+        return false; //!
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
index 0e21ca4..76e2206 100644
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -11,6 +11,7 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Angle;
@@ -119,4 +120,13 @@ public ChassisSpeeds getRobotVelocity() {
     public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
         swerveDrive.setChassisSpeeds(chassisSpeeds);
     }
+
+    public Rotation2d getHeading() {
+        return getPose().getRotation();
+    }
+
+    public ChassisSpeeds getTargetChassisSpeeds(double xInput, double yInput, Rotation2d angle) {
+        Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput));
+        return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), angle.getRadians(), getPose().getRotation().getRadians(), SwerveK.maxModuleSpeed.in(MetersPerSecond));                                        
+    }
 }