diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 177a46f..6fd15ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,8 +111,8 @@ public class RobotContainer { drivetrain.setDefaultCommand( drivetrain.drive( - () -> Math.pow(driver.getLeftY(), 3), - () -> Math.pow(driver.getLeftX(), 3), + () -> driver.getLeftY(), + () -> driver.getLeftX(), () -> driver.getRightX(), () -> true ) diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index a9b0552..3f828d2 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -9,4 +9,6 @@ public class OIConstants { public static final String kAutoTab = "Auto Tab"; public static final String kSensorsTab = "Sensors Tab"; public static final String kApriltagTab = "Apriltag Tab"; + + public static final double kJoystickExponential = 3; } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 89adee5..a47243c 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -323,8 +323,10 @@ public class Drivetrain extends SubsystemBase { */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond; - double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond; + double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); + + double xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + double ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed; var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(