From 5e784eb9f68616f264adb6876d522ed54d5c883b Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Thu, 11 Jan 2024 17:08:13 -0500 Subject: [PATCH] Adding rudimentary PathPlanner stuff --- src/main/java/frc/robot/RobotContainer.java | 16 +++++++- .../frc/robot/constants/AutoConstants.java | 14 +++++++ .../constants/SwerveModuleConstants.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 39 ++++++++++++++++++- vendordeps/PathplannerLib.json | 38 ++++++++++++++++++ 5 files changed, 105 insertions(+), 4 deletions(-) create mode 100644 vendordeps/PathplannerLib.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1c7caf..df6f25d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,10 +4,13 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.constants.OIConstants; import frc.robot.subsystems.Drivetrain; @@ -18,11 +21,20 @@ public class RobotContainer { private CommandXboxController primary; private CommandXboxController secondary; + private SendableChooser autoChooser; + public RobotContainer() { // TODO Need to provide a real initial pose // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all drivetrain = new Drivetrain(new Pose2d(), null); + // An example Named Command, doesn't need to remain once we start actually adding real things + // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration + NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); + + // TODO Specify a default auto string once we have one + autoChooser = AutoBuilder.buildAutoChooser(); + primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB); secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB); @@ -60,6 +72,6 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 3bfe98c..5896369 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -1,6 +1,11 @@ package frc.robot.constants; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; public final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 @@ -15,6 +20,15 @@ public final class AutoConstants { public static final double kPHeadingController = 0.02; // for heading control NOT PATHING public static final double kDHeadingController = 0.0025; + // TODO Is the robot module radius correct? + public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig( + new PIDConstants(kPXController), + new PIDConstants(kPThetaController), + kMaxSpeedMetersPerSecond, + Units.inchesToMeters(36.77), + new ReplanningConfig() + ); + // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); diff --git a/src/main/java/frc/robot/constants/SwerveModuleConstants.java b/src/main/java/frc/robot/constants/SwerveModuleConstants.java index 4d073bb..5dd374f 100644 --- a/src/main/java/frc/robot/constants/SwerveModuleConstants.java +++ b/src/main/java/frc/robot/constants/SwerveModuleConstants.java @@ -2,7 +2,7 @@ package frc.robot.constants; import com.revrobotics.CANSparkBase.IdleMode; -public class ModuleConstants { +public class SwerveModuleConstants { // The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. // This changes the drive speed of the module (a pinion gear with more teeth will result in a // robot that drives faster). diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 0bbfc2b..2378635 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -8,14 +8,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.WPIUtilJNI; +import java.util.Optional; import java.util.function.DoubleSupplier; import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; @@ -99,6 +102,22 @@ public class Drivetrain extends SubsystemBase { m_currentTranslationDir = 0.0; m_currentTranslationMag = 0.0; m_prevTime = WPIUtilJNI.now() * 1e-6; + + AutoBuilder.configureHolonomic( + this::getPose, + this::resetOdometry, + this::getCurrentChassisSpeeds, + this::driveWithChassisSpeeds, + AutoConstants.kPFConfig, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + ); } @Override @@ -157,6 +176,15 @@ public class Drivetrain extends SubsystemBase { ); } + public ChassisSpeeds getCurrentChassisSpeeds() { + return DrivetrainConstants.kDriveKinematics.toChassisSpeeds( + m_frontLeft.getState(), + m_frontRight.getState(), + m_rearLeft.getState(), + m_rearRight.getState() + ); + } + /** * Method to drive the robot using joystick info. * @@ -232,6 +260,15 @@ public class Drivetrain extends SubsystemBase { m_rearRight.setDesiredState(swerveModuleStates[3]); } + public void driveWithChassisSpeeds(ChassisSpeeds speeds) { + SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds); + + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_rearLeft.setDesiredState(states[2]); + m_rearRight.setDesiredState(states[3]); + } + public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) { return run(() -> { drive( diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..3c74146 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2024.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2024.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2024.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file