diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..4a40188 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..1cfeee2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,81 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8394326015162217, + "y": 4.188891299550061 + }, + "prevControl": null, + "nextControl": { + "x": 3.6904589462008053, + "y": 4.940665233394327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.78231056307781, + "y": 4.370352406171121 + }, + "prevControl": { + "x": 4.860483855805502, + "y": 5.074315409542372 + }, + "nextControl": { + "x": 7.141587777045881, + "y": 3.3323255596822756 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.085944870919574, + "y": 0.8637961970806275 + }, + "prevControl": { + "x": 6.4054472640776625, + "y": 0.7463117434053165 + }, + "nextControl": { + "x": 1.7664424777614869, + "y": 0.9812806507559387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8394326015162217, + "y": 4.188891299550061 + }, + "prevControl": { + "x": 2.3424992022139515, + "y": 3.371763469412239 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 85640e7..4d38688 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,11 @@ package frc.robot; +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 edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -33,6 +38,20 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { m_driveTrain.setDefaultCommand(new DriveUsingController(m_driveTrain, m_driverController)); + + AutoBuilder.configureHolonomic( + m_driveTrain::getPosition, + m_driveTrain::resetPosition, + m_driveTrain::getChassisSpeeds, + m_driveTrain::setChassisSpeeds, + new HolonomicPathFollowerConfig( + new PIDConstants(1.0, 0, 0), + new PIDConstants(1.0,0,0), + m_driveTrain.getMaxSpeed(), + m_driveTrain.getWheelBaseRadius(), + new ReplanningConfig() + ), + m_driveTrain); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 107aad7..3900825 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -5,13 +5,16 @@ package frc.robot.commands; import frc.robot.subsystems.ExampleSubsystem; + +import com.pathplanner.lib.commands.PathPlannerAuto; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; public final class Autos { /** Example static factory for an autonomous command. */ public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); + return new PathPlannerAuto("Example Auto"); } private Autos() { diff --git a/src/main/java/frc/robot/parameters/SwerveDriveParameters.java b/src/main/java/frc/robot/parameters/SwerveDriveParameters.java index 2ddf45c..4b712ad 100644 --- a/src/main/java/frc/robot/parameters/SwerveDriveParameters.java +++ b/src/main/java/frc/robot/parameters/SwerveDriveParameters.java @@ -261,6 +261,7 @@ public double getMaxRotationalSpeed() { return this.maxRotationalSpeed; } + /** * Returns the maximum rotational acceleration of the robot in rad/s^2. * @@ -270,6 +271,18 @@ public double getMaxRotationalAcceleration() { return this.maxRotationalAcceleration; } + /** + * Return the wheel base radius in meters. + * + * @return The wheel base radius in meters. + */ + public double getWheelBaseRadius(){ + double halfWheelDistanceX = wheelDistanceX/2; + double halfWheelDistanceY = wheelDistanceY/2; + + return Math.sqrt(halfWheelDistanceX*halfWheelDistanceX +halfWheelDistanceY*halfWheelDistanceY); + } + /** * Returns a {@link TrapezoidProfile.Constraints} object used to enforce * velocity and acceleration constraints on the {@link ProfilePIDController} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index afdeaa8..60c255c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -286,6 +286,15 @@ public TrapezoidProfile.Constraints getRotationalConstraints() { return PARAMETERS.getRotationalConstraints(); } + /** + * Return the wheel base radius in meters. + * + * @return The wheel base radius in meters. + */ + public double getWheelBaseRadius(){ + return PARAMETERS.getWheelBaseRadius(); + } + /** * Creates a HolonomicDriveController for the subsystem. *