Package com.stuypulse.robot.commands.swerve.PIDtoPose

Class SwerveDrivePIDToPose

java.lang.Object

edu.wpi.first.wpilibj2.command.Command

com.stuypulse.robot.commands.swerve.PIDtoPose.SwerveDrivePIDToPose

All Implemented Interfaces:

edu.wpi.first.util.sendable.Sendable

public class SwerveDrivePIDToPose
extends edu.wpi.first.wpilibj2.command.Command

Nested Class Summary

Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command

edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior

Field Summary

Fields

Modifier and Type	Field	Description
private Supplier<Boolean>	canEnd
private final edu.wpi.first.math.controller.HolonomicDriveController	controller
private final edu.wpi.first.math.filter.Debouncer	debounceRC
private final BooleanSupplier	isAligned
private boolean	isMotionProfiled
private final edu.wpi.first.math.filter.LinearFilter	lowPass
private double	maxAcceleration
private double	maxVelocity
private Number	maxVelocityWhenAligned
private final CommandSwerveDrivetrain	swerve
private final Supplier<edu.wpi.first.math.geometry.Pose2d>	targetPose
private final edu.wpi.first.wpilibj.smartdashboard.FieldObject2d	targetPose2d
private Number	thetaTolerance
private Supplier<edu.wpi.first.math.geometry.Translation2d>	translationSetpoint
private Number	xTolerance
private Number	yTolerance

Constructor Summary

Constructors

Constructor	Description
SwerveDrivePIDToPose(edu.wpi.first.math.geometry.Pose2d targetPose)
SwerveDrivePIDToPose(Supplier<edu.wpi.first.math.geometry.Pose2d> targetPose)

Method Summary

Modifier and Type	Method	Description
void	end(boolean interrupted)
void	execute()
private Supplier<edu.wpi.first.math.geometry.Translation2d>	getNewTranslationSetpointGenerator()
private double	getVelocityError()
void	initialize()
boolean	isAlignedTheta()
boolean	isAlignedX()
boolean	isAlignedY()
boolean	isFinished()
SwerveDrivePIDToPose	withCanEnd(Supplier<Boolean> canEnd)
SwerveDrivePIDToPose	withoutMotionProfile()
SwerveDrivePIDToPose	withTolerance(double x, double y, edu.wpi.first.math.geometry.Rotation2d theta)
SwerveDrivePIDToPose	withTranslationalConstraints(double maxVelocity, double maxAcceleration)

Methods inherited from class edu.wpi.first.wpilibj2.command.Command

addRequirements, addRequirements, alongWith, andThen, andThen, asProxy, beforeStarting, beforeStarting, cancel, deadlineFor, deadlineWith, finallyDo, finallyDo, getInterruptionBehavior, getName, getRequirements, getSubsystem, handleInterrupt, hasRequirement, ignoringDisable, initSendable, isScheduled, onlyIf, onlyWhile, raceWith, repeatedly, runsWhenDisabled, schedule, setName, setSubsystem, unless, until, withDeadline, withInterruptBehavior, withName, withTimeout, withTimeout

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Details

swerve

private final CommandSwerveDrivetrain swerve

controller

private final edu.wpi.first.math.controller.HolonomicDriveController controller

targetPose

private final Supplier<edu.wpi.first.math.geometry.Pose2d> targetPose

lowPass

private final edu.wpi.first.math.filter.LinearFilter lowPass

debounceRC

private final edu.wpi.first.math.filter.Debouncer debounceRC

maxVelocity

private double maxVelocity

maxAcceleration

private double maxAcceleration

isMotionProfiled

private boolean isMotionProfiled

isAligned

private final BooleanSupplier isAligned

targetPose2d

private final edu.wpi.first.wpilibj.smartdashboard.FieldObject2d targetPose2d

xTolerance

private Number xTolerance

yTolerance

private Number yTolerance

thetaTolerance

private Number thetaTolerance

maxVelocityWhenAligned

private Number maxVelocityWhenAligned

translationSetpoint

private Supplier<edu.wpi.first.math.geometry.Translation2d> translationSetpoint

canEnd

private Supplier<Boolean> canEnd

Constructor Details

SwerveDrivePIDToPose

public SwerveDrivePIDToPose(edu.wpi.first.math.geometry.Pose2d targetPose)

SwerveDrivePIDToPose

public SwerveDrivePIDToPose(Supplier<edu.wpi.first.math.geometry.Pose2d> targetPose)

Method Details

withTolerance

public SwerveDrivePIDToPose withTolerance(double x,
 double y,
 edu.wpi.first.math.geometry.Rotation2d theta)

withTranslationalConstraints

public SwerveDrivePIDToPose withTranslationalConstraints(double maxVelocity,
 double maxAcceleration)

withoutMotionProfile

public SwerveDrivePIDToPose withoutMotionProfile()

withCanEnd

public SwerveDrivePIDToPose withCanEnd(Supplier<Boolean> canEnd)

getNewTranslationSetpointGenerator

private Supplier<edu.wpi.first.math.geometry.Translation2d> getNewTranslationSetpointGenerator()

getVelocityError

private double getVelocityError()

initialize

public void initialize()

Overrides:

initialize in class edu.wpi.first.wpilibj2.command.Command

isAlignedX

public boolean isAlignedX()

isAlignedY

public boolean isAlignedY()

isAlignedTheta

public boolean isAlignedTheta()

execute

public void execute()

Overrides:

execute in class edu.wpi.first.wpilibj2.command.Command

isFinished

public boolean isFinished()

Overrides:

isFinished in class edu.wpi.first.wpilibj2.command.Command

end

public void end(boolean interrupted)

Overrides:

end in class edu.wpi.first.wpilibj2.command.Command