ControlledTrajectory

An extension of the LinearTrajectory that adds a way to control the speed of the robot at different points throughout the trajectory.

The controller you provide to instances of this class will have a target value of zero. The controller will attempt to minimize the distance the robot is from the target point.

Methods

ControlledTrajectory(PointXYZ target, Controller speedController, double tolerance, Angle angleTolerance)

Creates a new ControlledTrajectory. This constructor modifies the inputted controller by setting its minimum output to -1.0 and maximum output to 1.0.

• target: The trajectory's target point.
• speedController: The controller that controls the speed of the robot. This controller's input values are the distance the robot is from the target point.
• tolerance: The distance tolerance used in determining whether the robot's X and Y coordinates match up with those of the target point.
• angleTolerance: The angle tolerance used in determining whether the robot's heading matches up with the target heading.

speed(PointXYZ current)

Calculates and returns the speed at which the robot should move based on its current position and the speedController. The speed is determined by the distance to the target point, and the controller attempts to minimize this distance.