From 6cf8ca98c3c9c52ce34c085b266104c65b362c72 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Mon, 31 Jan 2022 19:29:00 -0700 Subject: [PATCH] Fix field relative rotation --- src/main/java/frc4388/robot/Constants.java | 3 ++- src/main/java/frc4388/robot/Robot.java | 15 ++++++++------- .../frc4388/robot/subsystems/SwerveDrive.java | 4 ++-- .../controller/DeadbandedRawXboxController.java | 4 ++-- .../controller/DeadbandedXboxController.java | 4 ++-- 5 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d7a8f0a..fc3f00b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -102,6 +102,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - public static final double AXIS_DEADBAND = 0.1; + public static final double LEFT_AXIS_DEADBAND = 0.1; + public static final double RIGHT_AXIS_DEADBAND = 0.6; } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index f313d27..edd849b 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -35,13 +35,14 @@ public class Robot extends TimedRobot { @Override public void robotInit() { if (org.fusesource.jansi.Ansi.isEnabled()) { - LOGGER.log(Level.ALL, "Logging Test 1/7"); - LOGGER.log(Level.WARNING, "Logging Test 2/7"); - LOGGER.log(Level.INFO, "Logging Test 3/7"); - LOGGER.log(Level.CONFIG, "Logging Test 4/7"); - LOGGER.log(Level.FINE, "Logging Test 5/7"); - LOGGER.log(Level.FINER, "Logging Test 6/7"); - LOGGER.log(Level.FINEST, "Logging Test 7/7"); + LOGGER.log(Level.ALL, "Logging Test 1/8"); + LOGGER.log(Level.SEVERE, "Logging Test 2/8"); + LOGGER.log(Level.WARNING, "Logging Test 3/8"); + LOGGER.log(Level.INFO, "Logging Test 4/8"); + LOGGER.log(Level.CONFIG, "Logging Test 5/8"); + LOGGER.log(Level.FINE, "Logging Test 6/8"); + LOGGER.log(Level.FINER, "Logging Test 7/8"); + LOGGER.log(Level.FINEST, "Logging Test 8/8"); } LOGGER.fine("robotInit()"); // Instantiate our RobotContainer. This will perform all our button bindings, and put our diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e891d19..b0402f4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -131,8 +131,8 @@ public class SwerveDrive extends SubsystemBase { 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); + if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) + rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java index 3ab9538..e1679dd 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -1,6 +1,6 @@ package frc4388.utility.controller; -import static frc4388.robot.Constants.OIConstants.AXIS_DEADBAND; +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; @@ -14,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); if (magnitude >= 1) return translation2d.div(magnitude); - if (magnitude < AXIS_DEADBAND) return new Translation2d(0,0); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 10ce42b..1c6fe5f 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -1,6 +1,6 @@ package frc4388.utility.controller; -import static frc4388.robot.Constants.OIConstants.AXIS_DEADBAND; +import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; @@ -15,7 +15,7 @@ public class DeadbandedXboxController extends XboxController { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); if (magnitude >= 1) return translation2d.div(magnitude); - if (magnitude < AXIS_DEADBAND) return new Translation2d(0,0); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } }