# TrajectoryBuilder Function List

# .forward(distance: Double)




 


// Drives forward 40 inches

new TrajectoryBuilder(new Pose2d())
  .forward(40)
  .build()

# .back(distance: Double)




 


// Drives backward 40 inches

new TrajectoryBuilder(new Pose2d())
  .back(40)
  .build()

# .strafeLeft(distance: Double)




 


// Strafes left 40 inches

new TrajectoryBuilder(new Pose2d())
  .strafeLeft(40)
  .build()

# .strafeRight(distance: Double)




 


// Strafes right 40 inches

new TrajectoryBuilder(new Pose2d())
  .strafeRight(40)
  .build()

# .strafeTo(endPosition: Vector2d)








 


// Robot moves to the specified coordinates.
// The robot maintains the heading it starts at throughout the trajectory
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

// strafeTo() is simply a shorthand for lineToConstantHeading()

new TrajectoryBuilder(new Pose2d())
  .strafeTo(new Vector2d(40, 40))
  .build()

# .lineTo(endPosition: Vector2d)








 


// Robot moves to the specified coordinates.
// The robot maintains the heading it starts at throughout the trajectory
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

// Functionally the same as strafeTo()

new TrajectoryBuilder(new Pose2d())
  .lineTo(new Vector2d(40, 40))
  .build()

# .lineToConstantHeading(endPosition: Vector2d)








 


// Robot moves to the specified coordinates.
// The robot maintains the heading it starts at throughout the trajectory
// So, if you start at a 90 degree angle, it will keep that angle the entire path.

// Functionally the same as strafeTo()/lineTo()

new TrajectoryBuilder(new Pose2d())
  .lineToConstantHeading(new Vector2d(40, 40))
  .build()

# .lineToLinearHeading(endPose: Pose2d)





 


// Robot moves to the specified coordinates while linearly
// interpolating between the start heading and a specified end heading.

new TrajectoryBuilder(new Pose2d())
  .lineToLinearHeading(new Pose2d(40, 40, Math.toRadians(90)))
  .build()

# .lineToSplineHeading(endPose: Pose2d)





 


// Robot moves to the specified coordinates while spline
// interpolating between the start heading and a specified end heading.

new TrajectoryBuilder(new Pose2d())
  .lineToSplineHeading(new Pose2d(40, 40, Math.toRadians(90)))
  .build()

# .splineTo(endPosition: Vector2d, endTangent: Double)





 


// Robot moves to the specified coordinates in a spline path
// while following a tangent heading interpolator.

new TrajectoryBuilder(new Pose2d())
  .splineTo(new Vector2d(40, 40), Math.toRadians(0))
  .build()

# .splineToConstantHeading(endPosition: Vector2d, endTangent: Double)







 


// Robot moves to the specified coordinates in a spline path
// while keeping the heading constant.
// The robot maintains the heading it starts at throughout the trajectory
// However, setting the `endTangent` does affect the spline shape.

new TrajectoryBuilder(new Pose2d())
  .splineToConstantHeading(new Vector2d(40, 40), Math.toRadians(0))
  .build()

# .splineToLinearHeading(endPose: Pose2d, endTangent: Double)














 


// Robot moves to the specified coordinates in a spline path
// while separately linearly interpolating the heading
//
// The heading interpolates to the heading specified in `endPose`.
// Setting `endTangent` affects the shape of the spline path itself.
//
// Due to the holonomic nature of mecanum drives, the bot is able
// to make such a movement while independently controlling heading.

// 🚨  Will cause PathContinuityException's!! 🚨
// Use splineToSplineHeading() if you are chaining these calls

new TrajectoryBuilder(new Pose2d())
  .splineToLinearHeading(new Pose2d(40, 40, Math.toRadians(90)), Math.toRadians(0))
  .build()

# .splineToSplineHeading(endPose: Pose2d, endTangent: Double)











 


// Robot moves to the specified coordinates in a spline path
// while separately spline interpolating the heading
//
// The heading interpolates to the heading specified in `endPose`.
// Setting `endTangent` affects the shape of the spline path itself.
//
// Due to the holonomic nature of mecanum drives, the bot is able
// to make such a movement while independently controlling heading.

new TrajectoryBuilder(new Pose2d())
  .splineToSplineHeading(new Pose2d(40, 40, Math.toRadians(90)), Math.toRadians(0))
  .build()