diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 12accfb0..20160249 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,7 +5,6 @@ package frc.robot; import com.pathplanner.lib.util.PIDConstants; - import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; @@ -24,13 +23,14 @@ public final class Constants public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. + public static final double MAX_SPEED = Units.feetToMeters(14.5); + // Maximum speed of the robot in meters per second, used to limit acceleration. public static final class AutonConstants { public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); - public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); + public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); } public static final class DrivebaseConstants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2fedde3a..d44e8b4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,13 +27,12 @@ public class RobotContainer { + // Replace with CommandPS4Controller or CommandJoystick if needed + final CommandXboxController driverXbox = new CommandXboxController(0); // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -42,8 +41,6 @@ public RobotContainer() // Configure the trigger bindings configureBindings(); - - // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive @@ -53,11 +50,11 @@ public RobotContainer() // WARNING: default buttons are on the same buttons as the ones defined in configureBindings AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), - OperatorConstants.LEFT_Y_DEADBAND), + OperatorConstants.LEFT_Y_DEADBAND), () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), - OperatorConstants.LEFT_X_DEADBAND), + OperatorConstants.LEFT_X_DEADBAND), () -> -MathUtil.applyDeadband(driverXbox.getRightX(), - OperatorConstants.RIGHT_X_DEADBAND), + OperatorConstants.RIGHT_X_DEADBAND), driverXbox.getHID()::getYButtonPressed, driverXbox.getHID()::getAButtonPressed, driverXbox.getHID()::getXButtonPressed, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 1513f2aa..a96c8112 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -36,6 +36,7 @@ import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; import swervelib.math.SwerveMath; +import swervelib.math.Vector2d; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; @@ -49,6 +50,10 @@ public class SwerveSubsystem extends SubsystemBase * Swerve drive object. */ private final SwerveDrive swerveDrive; + /** + * AprilTag field layout. + */ + private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); /** * Initialize {@link SwerveDrive} with the directory provided. @@ -131,11 +136,6 @@ public void setupPathPlanner() ); } - /** - * AprilTag field layout. - */ - private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - /** * Get the distance to the speaker. * @@ -252,10 +252,11 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat { // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { - double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out - double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out + Translation2d scaledInputs = new Vector2d(translationX.getAsDouble(), translationY.getAsDouble()).pow(3) + .getTranslation(); + // Make the robot move - driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, + driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), headingX.getAsDouble(), headingY.getAsDouble(), swerveDrive.getOdometryHeading().getRadians(), @@ -324,8 +325,9 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat { return run(() -> { // Make the robot move - swerveDrive.drive(new Translation2d(Math.pow(translationX.getAsDouble(), 3) * swerveDrive.getMaximumVelocity(), - Math.pow(translationY.getAsDouble(), 3) * swerveDrive.getMaximumVelocity()), + swerveDrive.drive(new Vector2d(new Translation2d(translationX.getAsDouble() * swerveDrive.getMaximumVelocity(), + translationY.getAsDouble() * + swerveDrive.getMaximumVelocity())).pow(3).getTranslation(), Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumAngularVelocity(), true, false); @@ -506,10 +508,9 @@ public Rotation2d getHeading() */ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) { - xInput = Math.pow(xInput, 3); - yInput = Math.pow(yInput, 3); - return swerveDrive.swerveController.getTargetSpeeds(xInput, - yInput, + Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation(); + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), headingX, headingY, getHeading().getRadians(), @@ -527,10 +528,10 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin */ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { - xInput = Math.pow(xInput, 3); - yInput = Math.pow(yInput, 3); - return swerveDrive.swerveController.getTargetSpeeds(xInput, - yInput, + Translation2d scaledInputs = new Vector2d(xInput, yInput).pow(3).getTranslation(); + + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), angle.getRadians(), getHeading().getRadians(), Constants.MAX_SPEED); diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index 85d9a45a..848852c6 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -360,7 +360,8 @@ public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog double velocity = module.getAbsoluteEncoder().getVelocity(); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power); SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle); - SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", velocity); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", + velocity); log.motor("angle-" + module.configuration.name) .voltage(m_appliedVoltage.mut_replace(power, Volts)) .angularPosition(m_anglePosition.mut_replace(angle, Degrees)) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 756ef79a..1e941790 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -379,8 +379,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint", desiredState.angle.getDegrees()); + SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint", + desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint", + desiredState.angle.getDegrees()); } } diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index a808d7af..dec51c1d 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -410,4 +410,16 @@ public static void antiJitter(SwerveModuleState moduleState, SwerveModuleState l moduleState.angle = lastModuleState.angle; } } + + /** + * Get the scaled {@link Translation2d} with the given scalar to change the magnitude of the {@link Vector2d}. + * + * @param cartesian {@link Translation2d} cartesian coordinates. + * @param scalar Scalar to change the polar radius of the {@link Vector2d} by. + * @return {@link Translation2d} scaled by the given input. + */ + public static Translation2d scaleTranslation2d(Translation2d cartesian, double scalar) + { + return new Vector2d(cartesian).scale(scalar).getTranslation(); + } } diff --git a/src/main/java/swervelib/math/Vector2d.java b/src/main/java/swervelib/math/Vector2d.java new file mode 100644 index 00000000..13d33b76 --- /dev/null +++ b/src/main/java/swervelib/math/Vector2d.java @@ -0,0 +1,96 @@ +package swervelib.math; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * Vector containing Magnitude and Position + */ +public class Vector2d +{ + + /** + * Position given as an angle {@link Rotation2d}. + */ + public final Rotation2d position; + /** + * Arbitrary magnitude of the vector. + */ + public final double magnitude; + + /** + * Construct a Vector with a position and magnitude of 0. + */ + public Vector2d() + { + position = new Rotation2d(); + magnitude = 0; + } + + /** + * Create a vector based off of the given {@link Translation2d}. The magnitude is the + * {@link Math#hypot(double, double)} of the X and Y. + * + * @param cartesian {@link Translation2d} with the X and Y set. + */ + public Vector2d(Translation2d cartesian) + { + position = cartesian.getAngle(); + magnitude = cartesian.getNorm(); + } + + /** + * Construct a {@link Vector2d} given the position and magnitude. + * + * @param position Position as a {@link Rotation2d}. + * @param magnitude Magnitude as a double. + */ + public Vector2d(Rotation2d position, double magnitude) + { + this.position = position; + this.magnitude = magnitude; + } + + /** + * Convert cartesian coordinates to Vector. + * + * @param x X position + * @param y Y position + */ + public Vector2d(double x, double y) + { + this(new Translation2d(x, y)); + } + + /** + * Convert the Vector back into cartesian coordinates. + * + * @return {@link Translation2d} of the vector. + */ + public Translation2d getTranslation() + { + return new Translation2d(magnitude, position); + } + + /** + * Scale the magnitude by the multiplier given + * + * @param scalar Multiplier of the magnitude. + * @return {@link Vector2d} for chained functions. + */ + public Vector2d scale(double scalar) + { + return new Vector2d(position, magnitude * scalar); + } + + /** + * Exponentially modify the magnitude. + * + * @param pow Power for the magnitude + * @return {@link Vector2d} with the magnitude^pow + */ + public Vector2d pow(double pow) + { + return new Vector2d(position, Math.pow(this.magnitude, pow)); + } +}