Package tagalong.commands.aim
Class RollerAimAtCmd<T extends TagalongSubsystemBase & RollerAugment>
java.lang.Object
edu.wpi.first.wpilibj2.command.Command
tagalong.commands.TagalongCommand
tagalong.commands.base.RollToDynamicCmd<T>
tagalong.commands.aim.RollerAimAtCmd<T>
- All Implemented Interfaces:
Sendable
public class RollerAimAtCmd<T extends TagalongSubsystemBase & RollerAugment>
extends RollToDynamicCmd<T>
Command that continuously points at a target in 2d space.
For rotational targeting give robot position as (X, Y) pose
For vertical targets give (distance from target, mechanism height)
-
Nested Class Summary
Nested classes/interfaces inherited from class edu.wpi.first.wpilibj2.command.Command
Command.InterruptionBehavior
-
Constructor Summary
ConstructorsConstructorDescriptionRollerAimAtCmd
(int id, T roller, Supplier<Translation2d> positionSupplier, Translation2d target) Minimal constructor with default parametersRollerAimAtCmd
(int id, T roller, Supplier<Translation2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.RollerAimAtCmd
(int id, T roller, Supplier<Translation2d> positionSupplier, Translation2d target, boolean holdPositionAfter, double maxVelocityRPS) Constructor that creates the command with the below parameters.RollerAimAtCmd
(T roller, Supplier<Translation2d> positionSupplier, Translation2d target) Minimal constructor with default parametersRollerAimAtCmd
(T roller, Supplier<Translation2d> positionSupplier, Translation2d target, boolean holdPositionAfter) Constructor that creates the command with the below parameters.RollerAimAtCmd
(T roller, Supplier<Translation2d> 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<Translation2d> 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
-
RollerAimAtCmd
public RollerAimAtCmd(int id, T roller, Supplier<Translation2d> 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
-
RollerAimAtCmd
Minimal constructor with default parameters- Parameters:
roller
- Tagalong Subsystem containing a roller microsystempositionSupplier
- Supplies the current robot position positiontarget
- Unmoving 2-dimensional target location
-
RollerAimAtCmd
public RollerAimAtCmd(int id, T roller, Supplier<Translation2d> 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
-
RollerAimAtCmd
public RollerAimAtCmd(T roller, Supplier<Translation2d> 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
-
RollerAimAtCmd
public RollerAimAtCmd(int id, T roller, Supplier<Translation2d> 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
-
RollerAimAtCmd
public RollerAimAtCmd(T roller, Supplier<Translation2d> 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<Translation2d> 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
-