From c6975fa81ea91475ecd54660dd150aacf5cdfb49 Mon Sep 17 00:00:00 2001
From: nathanrsxtn <nathanrsxtn@gmail.com>
Date: Sun, 30 Jan 2022 21:27:50 -0700
Subject: [PATCH] Add partial field relative chassis rotation

---
 .../java/frc4388/robot/RobotContainer.java    |  3 ++-
 .../frc4388/robot/subsystems/SwerveDrive.java | 26 +++++++++++++------
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 4429728..07805f6 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -69,7 +69,8 @@ public class RobotContainer {
         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
               getDriverController().getLeftX(),
               getDriverController().getLeftY(),
-              -getDriverController().getRightX(), 
+              getDriverController().getRightX(),
+              getDriverController().getRightY(),
           true),
            m_robotSwerveDrive));
 
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 7d9cd1e..e891d19 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -110,14 +110,6 @@ public class SwerveDrive extends SubsystemBase {
  */
   public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
   {
-    // Mechanism2d controllerMechanism = new Mechanism2d(2, 2);
-    // controllerMechanism.getRoot("Left Axes", speedX, speedY);
-    // double[] speedsClamped = clampJoystickAxes(speedX, speedY);
-    // double speedXClamped = speedsClamped[0];
-    // double speedYClamped = speedsClamped[1];
-    // controllerMechanism.getRoot("Left Axes (Clamped)", speedXClamped, speedYClamped);
-    // SmartDashboard.putData("Driver Controller", controllerMechanism);
-    // SmartDashboard.putNumberArray("Left Axes -> Left Axes (Clamped)", new double[] {speedX, speedY, speedXClamped, speedYClamped});
     if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
     else ignoreAngles = false;
     Translation2d speed = new Translation2d(speedX, speedY);
@@ -133,6 +125,24 @@ public class SwerveDrive extends SubsystemBase {
               : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
     setModuleStates(states);
   }
+  private Rotation2d rotTarget = new Rotation2d();
+  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
+  {
+    ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
+    Translation2d speed = new Translation2d(leftX, leftY);
+    speed = speed.times(speed.getNorm() * speedAdjust);
+    if (rightX > OIConstants.AXIS_DEADBAND || rightY > OIConstants.AXIS_DEADBAND)
+      rotTarget = new Rotation2d(rightX, -rightY);
+    double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
+    double xSpeedMetersPerSecond = -speed.getX();
+    double ySpeedMetersPerSecond = speed.getY();
+    SwerveModuleState[] states =
+        m_kinematics.toSwerveModuleStates(
+            fieldRelative
+              ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+              : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
+    setModuleStates(states);
+  }
 
   public void setModuleStates(SwerveModuleState[] desiredStates) {
     SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));