From a5f5fc29fd065382c0a7adf5146024b4c6bf68fa Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Thu, 14 Dec 2023 21:39:02 -0600 Subject: [PATCH 1/4] Adding Alternative Drive Mode for Rotation Shortcut Buttons and Right Joystick Adjustment Adding Alternative Drive Mode for Rotation Shortcut Buttons and Right Joystick Adjustment. Based off of ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172?u=technologyman00 --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 14 ++ .../drivebase/AbsoluteDriveAdv.java | 148 ++++++++++++++++++ 3 files changed, 164 insertions(+) create mode 100644 src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ff6acd795..d7bdc496b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,5 +48,7 @@ public static class OperatorConstants // Joystick Deadband public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; + public static final double RIGHT_X_DEADBAND = 0.01; + public static final double TURN_CONSTANT = 0.25; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abfd70f63..7deadf4b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.commands.swervedrive.auto.Autos; import frc.robot.commands.swervedrive.drivebase.AbsoluteDrive; import frc.robot.commands.swervedrive.drivebase.AbsoluteFieldDrive; +import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; @@ -66,6 +67,19 @@ public RobotContainer() () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); + + AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), + OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), + OperatorConstants.LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getRightX(), + OperatorConstants.RIGHT_X_DEADBAND), + driverXbox.getYButton(), + driverXbox.getBButton(), + driverXbox.getXButton(), + driverXbox.getAButton()); + TeleopDrive simClosedFieldRel = new TeleopDrive(drivebase, () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java new file mode 100644 index 000000000..59a3ed379 --- /dev/null +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -0,0 +1,148 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.swervedrive.drivebase; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import java.util.List; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** + * A more advanced Swerve Control System that has 4 buttons for which direction to face + */ +public class AbsoluteDriveAdv extends CommandBase +{ + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private boolean initRotation = false; + private final boolean lookAway, lookTowards, lookLeft, lookRight; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is + * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being + * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. + * Based off of ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 + * to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 + * to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be adjusted. + * Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, boolean lookAway, + boolean lookTowards, boolean lookLeft, boolean lookRight) + { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() + { + initRotation = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() + { + double headingX = 0; + double headingY = 0; + double newHeading = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if(lookAway){ + headingX = 1; + } + // Face Right + if(lookRight){ + headingY = 1; + } + // Face Left + if(lookLeft){ + headingY = -1; + } + // Face Towards the Drivers + if(lookTowards){ + headingX = -1; + } + + //Dont overwrite a button press + if(headingX == 0 && headingY == 0){ + newHeading = swerve.getHeading().getRadians() + (Constants.OperatorConstants.TURN_CONSTANT * headingAdjust.getAsDouble()); + headingX = Math.sin(newHeading); + headingY = Math.cos(newHeading); + } + + ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), + headingX, + headingY); + + // Prevent Movement After Auto + if(initRotation) + { + if(headingX == 0 && headingY == 0) + { + // Get the curretHeading + double firstLoopHeading = swerve.getHeading().getRadians(); + + // Set the Current Heading to the desired Heading + desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); + } + //Dont Init Rotation Again + initRotation = false; + } + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), + Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) + { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() + { + return false; + } + + +} From cd782ce058e5d22f155fed155bf08ebe2d29932d Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Fri, 15 Dec 2023 09:17:13 -0600 Subject: [PATCH 2/4] Changed to use Rotation2d for Angle Math Previously used doubles to pass around the angles, this is fine, but preferred to use Rotation2d instead. This way any angle plus or minus or cos and sin conversions are handled better --- .../swervedrive/drivebase/AbsoluteDrive.java | 5 +++-- .../drivebase/AbsoluteDriveAdv.java | 20 ++++++++++--------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index 00dfbd282..42c42ded2 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -4,6 +4,7 @@ package frc.robot.commands.swervedrive.drivebase; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -80,10 +81,10 @@ public void execute() if(headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { // Get the curretHeading - double firstLoopHeading = swerve.getHeading().getRadians(); + Rotation2d firstLoopHeading = swerve.getHeading(); // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); + desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); } //Dont Init Rotation Again initRotation = false; diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 59a3ed379..381caeaca 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -4,6 +4,7 @@ package frc.robot.commands.swervedrive.drivebase; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -46,8 +47,8 @@ public class AbsoluteDriveAdv extends CommandBase * @param lookLeft Face the robot left * @param lookRight Face the robot right */ - public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, boolean lookAway, - boolean lookTowards, boolean lookLeft, boolean lookRight) + public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, + boolean lookAway, boolean lookTowards, boolean lookLeft, boolean lookRight) { this.swerve = swerve; this.vX = vX; @@ -73,7 +74,7 @@ public void execute() { double headingX = 0; double headingY = 0; - double newHeading = 0; + Rotation2d newHeading = Rotation2d.fromRadians(0); // These are written to allow combinations for 45 angles // Face Away from Drivers @@ -95,9 +96,10 @@ public void execute() //Dont overwrite a button press if(headingX == 0 && headingY == 0){ - newHeading = swerve.getHeading().getRadians() + (Constants.OperatorConstants.TURN_CONSTANT * headingAdjust.getAsDouble()); - headingX = Math.sin(newHeading); - headingY = Math.cos(newHeading); + newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * headingAdjust.getAsDouble()) + .plus(swerve.getHeading()); + headingX = newHeading.getSin(); + headingY = newHeading.getCos(); } ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), @@ -110,10 +112,10 @@ public void execute() if(headingX == 0 && headingY == 0) { // Get the curretHeading - double firstLoopHeading = swerve.getHeading().getRadians(); - + Rotation2d firstLoopHeading = swerve.getHeading(); + // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, Math.sin(firstLoopHeading), Math.cos(firstLoopHeading)); + desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); } //Dont Init Rotation Again initRotation = false; From 9f144dc3ee8971d0d96b0ce8e4993a079fcc57ec Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Fri, 15 Dec 2023 13:06:52 -0600 Subject: [PATCH 3/4] Changed to Suppliers and Fixed Wrong button order Should have used suppliers for passing references and the button order was incorrect. --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../swervedrive/drivebase/AbsoluteDriveAdv.java | 14 ++++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7deadf4b4..25d108e07 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,10 +75,10 @@ public RobotContainer() OperatorConstants.LEFT_X_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), - driverXbox.getYButton(), - driverXbox.getBButton(), - driverXbox.getXButton(), - driverXbox.getAButton()); + driverXbox::getYButton, + driverXbox::getAButton, + driverXbox::getXButton, + driverXbox::getBButton); TeleopDrive simClosedFieldRel = new TeleopDrive(drivebase, () -> MathUtil.applyDeadband(driverXbox.getLeftY(), diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 381caeaca..ebab8b5b9 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -7,11 +7,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.util.List; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import swervelib.SwerveController; import swervelib.math.SwerveMath; @@ -26,7 +28,7 @@ public class AbsoluteDriveAdv extends CommandBase private final DoubleSupplier vX, vY; private final DoubleSupplier headingAdjust; private boolean initRotation = false; - private final boolean lookAway, lookTowards, lookLeft, lookRight; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; /** * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is @@ -48,7 +50,7 @@ public class AbsoluteDriveAdv extends CommandBase * @param lookRight Face the robot right */ public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, - boolean lookAway, boolean lookTowards, boolean lookLeft, boolean lookRight) + BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft, BooleanSupplier lookRight) { this.swerve = swerve; this.vX = vX; @@ -78,19 +80,19 @@ public void execute() // These are written to allow combinations for 45 angles // Face Away from Drivers - if(lookAway){ + if(lookAway.getAsBoolean()){ headingX = 1; } // Face Right - if(lookRight){ + if(lookRight.getAsBoolean()){ headingY = 1; } // Face Left - if(lookLeft){ + if(lookLeft.getAsBoolean()){ headingY = -1; } // Face Towards the Drivers - if(lookTowards){ + if(lookTowards.getAsBoolean()){ headingX = -1; } From 20df7780b04f9b19989b43a68a3f4c15d34e390a Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Fri, 15 Dec 2023 15:07:21 -0600 Subject: [PATCH 4/4] Quick Rotate Buttons no longer need to be held and 45s are a bit easier After simulation testing AbsoluteDriveAdv it need a bit of tuning as the robot turned the wrong direction. Also the quick rotate buttons would have to be held down previously as the joystick adjustment would overwrite them as it would always be changing with the gyro. I also changed the default constant as it was unbareably slow. --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../commands/swervedrive/drivebase/AbsoluteDriveAdv.java | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d7bdc496b..3be8a30ff 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -49,6 +49,6 @@ public static class OperatorConstants public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; public static final double RIGHT_X_DEADBAND = 0.01; - public static final double TURN_CONSTANT = 0.25; + public static final double TURN_CONSTANT = 0.75; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25d108e07..7ab7eaf87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,10 +75,10 @@ public RobotContainer() OperatorConstants.LEFT_X_DEADBAND), () -> MathUtil.applyDeadband(driverXbox.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), - driverXbox::getYButton, - driverXbox::getAButton, - driverXbox::getXButton, - driverXbox::getBButton); + driverXbox::getYButtonPressed, + driverXbox::getAButtonPressed, + driverXbox::getXButtonPressed, + driverXbox::getBButtonPressed); TeleopDrive simClosedFieldRel = new TeleopDrive(drivebase, () -> MathUtil.applyDeadband(driverXbox.getLeftY(), diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index ebab8b5b9..235ab5d7e 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -97,8 +97,8 @@ public void execute() } //Dont overwrite a button press - if(headingX == 0 && headingY == 0){ - newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * headingAdjust.getAsDouble()) + if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){ + newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()) .plus(swerve.getHeading()); headingX = newHeading.getSin(); headingY = newHeading.getCos();