From b4bf4296de24de8d1bc030b820e3b40d01b209e8 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 17 Mar 2022 15:09:46 -0600
Subject: [PATCH] driving field relative works and nathans turning works

---
 src/main/java/frc4388/robot/Constants.java           |  2 +-
 src/main/java/frc4388/robot/RobotContainer.java      |  2 +-
 .../java/frc4388/robot/subsystems/SwerveDrive.java   | 12 +++++++-----
 3 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 5376fd2..549a128 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -34,7 +34,7 @@ import frc4388.utility.LEDPatterns;
  */
 public final class Constants {
   public static final class SwerveDriveConstants {
-    public static final double ROTATION_SPEED = 2.0;
+    public static final double ROTATION_SPEED = 4.0;
     public static final double WIDTH = 23.5;
     public static final double HEIGHT = 23.5;
     public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index b028826..559fcaf 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -155,7 +155,7 @@ public class RobotContainer {
             getDriverController().getLeftY(),
             //getDriverController().getRightX(),
             getDriverController().getRightX(),
-            // getDriverController().getRightY(),
+            getDriverController().getRightY(),
             true),
             m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
       // Intake with Triggers
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 262c950..e1b8fae 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -113,16 +113,16 @@ public class SwerveDrive extends SubsystemBase {
 
   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);
+    Translation2d speed = new Translation2d(-leftX, leftY);
     speed = speed.times(speed.getNorm() * speedAdjust);
     if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
-      rotTarget = new Rotation2d(rightY, rightX);
+      rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
     double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
-    double xSpeedMetersPerSecond = speed.getX();
+    double xSpeedMetersPerSecond = -speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
         ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-            rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees()))
+            rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2)))
         : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8);
     SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
         chassisSpeeds);
@@ -154,6 +154,8 @@ public class SwerveDrive extends SubsystemBase {
     updateOdometry();
     updateSmartDash();
 
+    SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
+    SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
     SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
 
@@ -165,7 +167,7 @@ public class SwerveDrive extends SubsystemBase {
     // odometry
     SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
     SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
-    SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
+    SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
 
     // chassis speeds
     // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds