From 0d3c48454d755dbeb88995f042eab953dea7f3ad Mon Sep 17 00:00:00 2001
From: Jack Lingle <captainjack0818@gmail.com>
Date: Mon, 18 Nov 2024 15:51:15 -0500
Subject: [PATCH] swap x and y in rotation 2d and add configurable deadband in
 constructor

---
 .../wpilib_interface/DriveWithJoysticks.java  | 21 ++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java b/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java
index c738e4c..a83f44a 100644
--- a/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java
+++ b/wpilib_interface/src/main/java/coppercore/wpilib_interface/DriveWithJoysticks.java
@@ -15,18 +15,21 @@ public class DriveWithJoysticks extends Command {
     private CommandJoystick rightJoystick;
     private double maxLinearVelocity;
     private double maxAngularVelocity;
+    private double joystickDeadband;
 
     public DriveWithJoysticks(
             DriveTemplate drive,
             CommandJoystick leftJoystick,
             CommandJoystick rightJoystick,
             double maxLinearVelocity,
-            double maxAngularVelocity) {
+            double maxAngularVelocity,
+            double joystickDeadband) {
         this.drive = drive;
         this.leftJoystick = leftJoystick;
         this.rightJoystick = rightJoystick;
         this.maxLinearVelocity = maxLinearVelocity;
         this.maxAngularVelocity = maxAngularVelocity;
+        this.joystickDeadband = joystickDeadband;
 
         addRequirements(this.drive);
     }
@@ -35,7 +38,7 @@ public DriveWithJoysticks(
     public void execute() {
         Translation2d linearSpeeds = getLinearVelocity(-leftJoystick.getX(), -leftJoystick.getY());
 
-        double omega = Deadband.oneAxisDeadband(-rightJoystick.getX(), 0.1);
+        double omega = Deadband.oneAxisDeadband(-rightJoystick.getX(), joystickDeadband);
         omega = Math.copySign(omega * omega, omega);
 
         drive.setGoalSpeeds(
@@ -46,11 +49,19 @@ public void execute() {
                 true);
     }
 
+    /* returns a calculated translation with squared velocity */
     public Translation2d getLinearVelocity(double x, double y) {
-        double deadband = Deadband.oneAxisDeadband(Math.hypot(x, y), 0.1);
+        double[] deadbands = Deadband.twoAxisDeadband(x, y, joystickDeadband); 
 
-        Rotation2d direction = new Rotation2d(x, y);
-        double squaredMagnitude = deadband * deadband;
+        double x = deadbands[0], y = deadbands[1];
+        double magnitude = Math.hypot(x, y);
+
+        /* joystick x/y is opposite of field x/y
+         * therefore, x and y must be flipped for proper rotation of pose
+         * BEWARE: not flipping will cause forward on joystick to drive right on field
+         */
+        Rotation2d direction = new Rotation2d(y, x);
+        double squaredMagnitude = magnitude * magnitude;
 
         Translation2d linearVelocity =
                 new Pose2d(new Translation2d(), direction)