Adding rudimentary PathPlanner stuff

This commit is contained in:
Bradley Bickford 2024-01-11 17:08:13 -05:00
parent 69fd4b76cc
commit 5e784eb9f6
5 changed files with 105 additions and 4 deletions

View File

@ -4,10 +4,13 @@
package frc.robot; 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.MathUtil;
import edu.wpi.first.math.geometry.Pose2d; 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.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.constants.OIConstants; import frc.robot.constants.OIConstants;
import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Drivetrain;
@ -18,11 +21,20 @@ public class RobotContainer {
private CommandXboxController primary; private CommandXboxController primary;
private CommandXboxController secondary; private CommandXboxController secondary;
private SendableChooser<Command> autoChooser;
public RobotContainer() { public RobotContainer() {
// TODO Need to provide a real initial pose // TODO Need to provide a real initial pose
// TODO Need to provide a real VisualPoseProvider, null means we're not using one at all // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
drivetrain = new Drivetrain(new Pose2d(), null); 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); primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB); secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
@ -60,6 +72,6 @@ public class RobotContainer {
} }
public Command getAutonomousCommand() { public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured"); return autoChooser.getSelected();
} }
} }

View File

@ -1,6 +1,11 @@
package frc.robot.constants; 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.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
public final class AutoConstants { public final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 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 kPHeadingController = 0.02; // for heading control NOT PATHING
public static final double kDHeadingController = 0.0025; 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 // Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);

View File

@ -2,7 +2,7 @@ package frc.robot.constants;
import com.revrobotics.CANSparkBase.IdleMode; 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. // 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 // This changes the drive speed of the module (a pinion gear with more teeth will result in a
// robot that drives faster). // robot that drives faster).

View File

@ -8,14 +8,17 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 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.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.util.WPIUtilJNI;
import java.util.Optional;
import java.util.function.DoubleSupplier; import java.util.function.DoubleSupplier;
import com.kauailabs.navx.frc.AHRS; 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 edu.wpi.first.wpilibj.SPI;
import frc.robot.constants.AutoConstants; import frc.robot.constants.AutoConstants;
import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.DrivetrainConstants;
@ -99,6 +102,22 @@ public class Drivetrain extends SubsystemBase {
m_currentTranslationDir = 0.0; m_currentTranslationDir = 0.0;
m_currentTranslationMag = 0.0; m_currentTranslationMag = 0.0;
m_prevTime = WPIUtilJNI.now() * 1e-6; m_prevTime = WPIUtilJNI.now() * 1e-6;
AutoBuilder.configureHolonomic(
this::getPose,
this::resetOdometry,
this::getCurrentChassisSpeeds,
this::driveWithChassisSpeeds,
AutoConstants.kPFConfig,
() -> {
Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this
);
} }
@Override @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. * Method to drive the robot using joystick info.
* *
@ -232,6 +260,15 @@ public class Drivetrain extends SubsystemBase {
m_rearRight.setDesiredState(swerveModuleStates[3]); 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) { public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
return run(() -> { return run(() -> {
drive( drive(

View File

@ -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"
]
}
]
}