From 7d9780053683408143c39da0dba681ce9d37492c Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Tue, 23 Jul 2024 09:15:37 -0600
Subject: [PATCH] talon stuff

---
 src/main/java/frc4388/robot/RobotContainer.java       |  4 ++--
 .../java/frc4388/robot/subsystems/SwerveDrive.java    |  4 +++-
 .../java/frc4388/robot/subsystems/SwerveModule.java   | 11 +++++++----
 .../utility/controller/DeadbandedXboxController.java  |  1 +
 4 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c7b21a3..c9b33d3 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -182,8 +182,8 @@ public class RobotContainer {
             // ! Swerve Drive Default Command (Regular Rotation)
             m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
                 m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getRightTrigger(),
-                                                getDeadbandedDriverController().getRight(),
-                                    true);
+                                                getDeadbandedDriverController().getRight().times(-1),
+                                    false);
             }, m_robotSwerveDrive)
             .withName("SwerveDrive DefaultCommand"));
             m_robotSwerveDrive.hellsHorses();
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index bd5e344..f66cd67 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -84,7 +84,9 @@ public class  SwerveDrive extends SubsystemBase {
       // Convert field-relative speeds to robot-relative speeds.
       chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
     } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+
+      chassisSpeeds = new ChassisSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
     setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 8893fcf..0c56482 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -63,15 +64,17 @@ public class SwerveModule extends SubsystemBase {
     
     @Override
     public void periodic() {
+        if (DriverStation.isEnabled()) {
+            System.out.println("Drive Velocity_60FT_NoRunup: " + selfid + ", " + getDriveVel());        
+            System.out.println("Drive Powerdraw_60FT_NoRunup: " + selfid + ", " + driveMotor.getMotorOutputVoltage());
+        }
         //encoder.configMagnetOffset(offsetGetter.get());
         //SmartDashboard.putString("Error Code: " + selfid, getstuff());
         // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
         // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
         // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
-        SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());        
-        SmartDashboard.putNumber("Drive Powerdraw: " + selfid, driveMotor.getMotorOutputVoltage());
-        System.out.println("Drive Velocity: " + selfid + ", " + getDriveVel());        
-        System.out.println("Drive Powerdraw: " + selfid + ", " + driveMotor.getMotorOutputVoltage());
+        // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());        
+        // SmartDashboard.putNumber("Drive Powerdraw: " + selfid, driveMotor.getMotorOutputVoltage());
 
     }
     /**
diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
index bab6c8e..e6d0ff0 100644
--- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
@@ -16,6 +16,7 @@ public class DeadbandedXboxController extends XboxController {
   public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
   public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
   public Translation2d getRightTrigger() {return skewToDeadzonedCircle(0, -getRightTriggerAxis() + getLeftTriggerAxis());}
+  public Translation2d getFixed() {return skewToDeadzonedCircle(0, -0.8);}
   public static Translation2d skewToDeadzonedCircle(double x, double y) {
     Translation2d translation2d = new Translation2d(x, y);
     double magnitude = translation2d.getNorm();