Adding rudimentary PathPlanner stuff

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

View File

@@ -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);

View File

@@ -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).