diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index a47243c..b513f24 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -324,9 +324,17 @@ 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 p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); + double xSpeedDelivered = 0; + double ySpeedDelivered = 0; - double xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; - double ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + if(p != 0){ + xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond; + }else{ + xSpeedDelivered = 0; + ySpeedDelivered = 0; + } + double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed; var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(