Package tagalong.commands.aim
Class RollerAimAtYawCompCmd<T extends TagalongSubsystemBase & RollerAugment>
java.lang.Object
edu.wpi.first.wpilibj2.command.Command
tagalong.commands.TagalongCommand
tagalong.commands.base.RollToDynamicCmd<T>
tagalong.commands.aim.RollerAimAtYawCompCmd<T>
- All Implemented Interfaces:
Sendable
public class RollerAimAtYawCompCmd<T extends TagalongSubsystemBase & RollerAugment>
extends RollToDynamicCmd<T>
Command that continuously points at a target in 2d space while compensating for the current
rotation of the robot. Since this is strictly rotational targeting give robot position as (X, Y)
pose
-
Nested Class Summary
Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command
Command.InterruptionBehavior
-
Constructor Summary
ConstructorsConstructorDescriptionRollerAimAtYawCompCmd
(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target) Minimal constructor with default parametersRollerAimAtYawCompCmd
(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.RollerAimAtYawCompCmd
(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter, double maxVelocityRPS) Constructor that creates the command with the below parameters.RollerAimAtYawCompCmd
(T roller, Supplier<Pose2d> positionSupplier, Translation2d target) Minimal constructor with default parametersRollerAimAtYawCompCmd
(T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.RollerAimAtYawCompCmd
(T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter, double maxVelocityRPS) Constructor that creates the command with the below parameters. -
Method Summary
Modifier and TypeMethodDescriptionprotected static DoubleSupplier
positionTransform
(Supplier<Pose2d> location, DoubleSupplier rollerPosition, Translation2d target) Transform position into an optimal path angle to target, ignores yawMethods inherited from class tagalong.commands.base.RollToDynamicCmd
end, execute, initialize, isFinished
Methods inherited from class tagalong.commands.TagalongCommand
anonymize
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
-
Constructor Details
-
RollerAimAtYawCompCmd
public RollerAimAtYawCompCmd(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target) Minimal constructor with default parameters- Parameters:
id
- Integer ID of the roller microsystem inside the Tagalong Subsystemroller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target location
-
RollerAimAtYawCompCmd
Minimal constructor with default parameters- Parameters:
roller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target location
-
RollerAimAtYawCompCmd
public RollerAimAtYawCompCmd(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter, double maxVelocityRPS) Constructor that creates the command with the below parameters.- Parameters:
id
- Integer ID of the roller microsystem inside the Tagalong Subsystemroller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target locationholdPositionAfter
- If the roller should hold position when the command completesmaxVelocityRPS
- The maximum velocity of the roller, in rotations per second, during this command
-
RollerAimAtYawCompCmd
public RollerAimAtYawCompCmd(T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter, double maxVelocityRPS) Constructor that creates the command with the below parameters.- Parameters:
roller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target locationholdPositionAfter
- If the roller should hold position when the command completesmaxVelocityRPS
- The maximum velocity of the roller, in rotations per second, during this command
-
RollerAimAtYawCompCmd
public RollerAimAtYawCompCmd(int id, T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.- Parameters:
id
- Integer ID of the roller microsystem inside the Tagalong Subsystemroller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target locationholdPositionAfter
- If the roller should hold position when the command completes
-
RollerAimAtYawCompCmd
public RollerAimAtYawCompCmd(T roller, Supplier<Pose2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.- Parameters:
roller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target locationholdPositionAfter
- If the roller should hold position when the command completes
-
-
Method Details
-
positionTransform
protected static DoubleSupplier positionTransform(Supplier<Pose2d> location, DoubleSupplier rollerPosition, Translation2d target) Transform position into an optimal path angle to target, ignores yaw- Parameters:
location
- Current robot location supplierrollerPosition
- Roller position suppliertarget
- Target to aim at's location- Returns:
- Optimal path target angle for the roller
-