From c77a432e595b1e1fe235b6065ea55ca89e14c026 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Thu, 11 Jan 2024 19:23:30 -0800 Subject: [PATCH] should have written configuration code for swerve. Everything should work :) --- .../swerve/controllers/SwerveModule.java | 4 +-- .../controllers/drive/DriveController.java | 2 +- .../controllers/steer/SteerConfiguration.java | 2 +- .../java/com/team2813/subsystems/Drive.java | 32 +++++++++++++++++++ 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/SwerveModule.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/SwerveModule.java index 9dcd48c..4bb688c 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/SwerveModule.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/SwerveModule.java @@ -1,7 +1,7 @@ -package com.team2813.lib.swerve.controllers; +package com.team2813.lib2813.swerve.controllers; import com.swervedrivespecialties.swervelib.SteerController; -import com.team2813.lib.swerve.controllers.drive.DriveController; +import com.team2813.lib2813.swerve.controllers.drive.DriveController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/DriveController.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/DriveController.java index e172934..3d894c1 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/DriveController.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/DriveController.java @@ -1,4 +1,4 @@ -package com.team2813.lib.swerve.controllers.drive; +package com.team2813.lib2813.swerve.controllers.drive; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java index ed32526..34707e0 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java @@ -1,4 +1,4 @@ -package com.team2813.lib.swerve.controllers.steer; +package com.team2813.lib2813.swerve.controllers.steer; import com.swervedrivespecialties.swervelib.ctre.CanCoderAbsoluteConfiguration; diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java index 288ff32..8d68f57 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java @@ -1,6 +1,10 @@ package com.team2813.subsystems; import com.ctre.phoenix.sensors.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import com.swervedrivespecialties.swervelib.Mk4iSwerveModuleHelper.GearRatio; import com.swervedrivespecialties.swervelib.SdsModuleConfigurations; @@ -12,6 +16,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -127,8 +133,34 @@ public Drive() { ); pigeon.configMountPose(Pigeon2.AxisDirection.PositiveY, Pigeon2.AxisDirection.PositiveZ); + + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getChassisSpeeds, + this::drive, + new HolonomicPathFollowerConfig( + new PIDConstants(0), + new PIDConstants(0), + MAX_VELOCITY, + 0.4, + new ReplanningConfig() + ), + Drive::onRed, + this); } + /** + * A method that gets whether you are on red alliance. If there is not an alliance, + * returns false + * @return {@code true} if you are on the red alliance + */ + private static boolean onRed() { + return DriverStation.getAlliance() + .map((j) -> j == Alliance.Red) + .orElse(false); + } + public Rotation2d getRotation() { return Rotation2d.fromDegrees(pigeon.getHeading()); }