From d3d715780289d02a248806356113534d350e1e40 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Mon, 4 Apr 2022 13:37:20 -0600 Subject: [PATCH] Reduce joystick skew calculations a bit --- .../java/frc4388/robot/RobotContainer.java | 36 ++++++++----------- .../robot/commands/ShooterCommands/Shoot.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 33 ++++++++++------- .../DeadbandedRawXboxController.java | 10 +++--- .../controller/DeadbandedXboxController.java | 11 +++--- 5 files changed, 49 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 79e106a..d4a4e3a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -96,8 +96,8 @@ public class RobotContainer { private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom); // Controllers - private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); public static boolean softLimits = true; @@ -225,25 +225,19 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ - // Swerve Drive with Input - m_robotSwerveDrive.setDefaultCommand( + // Swerve Drive with Input + m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (currentDriveMode.equals(DriveMode.ON)) { - m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true); } - if (currentDriveMode.equals(DriveMode.OFF)) { - m_robotSwerveDrive.driveWithInput( 0, - 0, - 0, - 0, - false); - }} - , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + if (currentDriveMode == DriveMode.ON) { + m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), + getDriverController().getRight(), true); + } + if (currentDriveMode == DriveMode.OFF) { + m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false); + } + }, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // Intake with Triggers + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), @@ -631,11 +625,11 @@ public class RobotContainer { } } - public static XboxController getDriverController() { + public static DeadbandedXboxController getDriverController() { return m_driverXbox; } - public XboxController getOperatorController() { + public static DeadbandedXboxController getOperatorController() { return m_operatorXbox; } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 906d05d..e255e4b 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -186,7 +186,7 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Normalized Output", this.normOutput); this.turret.runTurretWithCustomPID(normOutput); - this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative + this.swerve.driveWithInput(RobotContainer.getDriverController().getLeft(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index beeb4d2..e0c1994 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -81,6 +81,11 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putData("Field", m_field); } + public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { + Translation2d speed = new Translation2d(speedX, speedY); + driveWithInput(speed, rot, fieldRelative); + } + /** * Method to drive the robot using joystick info. * @link https://github.com/ZachOrr/MK3-Swerve-Example @@ -90,10 +95,9 @@ public class SwerveDrive extends SubsystemBase { * @param fieldRelative Whether the provided x and y speeds are relative to the * field. */ - public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { - ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0); - - Translation2d speed = new Translation2d(speedX, speedY); + public void driveWithInput(Translation2d speed, double rot, boolean fieldRelative) { + ignoreAngles = (speed.getX() == 0) && (speed.getY() == 0) && (rot == 0); + double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); @@ -108,23 +112,28 @@ public class SwerveDrive extends SubsystemBase { setModuleStates(states); } - // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) 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); - speed = speed.times(speed.getNorm() * speedAdjust); - 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)); + Translation2d head = new Translation2d(rightX, rightY); + driveWithInput(speed, head, fieldRelative); + } + + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + ignoreAngles = leftStick.getX() == 0 && leftStick.getY() == 0 && rightStick.getX() == 0 && rightStick.getY() == 0; + leftStick = leftStick.times(leftStick.getNorm() * speedAdjust); + if (Math.abs(rightStick.getX()) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightStick.getY()) > OIConstants.RIGHT_AXIS_DEADBAND) + rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); if (ignoreAngles) { rot = 0; } - double xSpeedMetersPerSecond = speed.getX(); - double ySpeedMetersPerSecond = speed.getY(); + double xSpeedMetersPerSecond = leftStick.getX(); + double ySpeedMetersPerSecond = leftStick.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED * 2, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 2); + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED * 2); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); // if (ignoreAngles) { diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java index 177ef4e..4cb2e76 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -7,10 +7,12 @@ import frc4388.robot.Constants.OIConstants; public class DeadbandedRawXboxController extends RawXboxController { public DeadbandedRawXboxController(int port) { super(port); } - @Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); } - @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); } - @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); } - @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); } + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()); } public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index f883fd3..999c164 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -8,11 +8,12 @@ import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } - - @Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); } - @Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); } - @Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); } - @Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); } + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()); } public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm();