TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RingPickupPathGenerator.java
/**
     * Generate trajectory from startPos to ring 1, ring 2, ring 3 to endPos
     * @param r1 Ring 1 coordinate
     * @param r2 Ring 2 coordinate
     * @param r3 Ring 3 coordinate
     * @return Trajectory
     */
Generate trajectory from startPos to ring 1, ring 2, ring 3 to endPos
@param r1 Ring 1 coordinate
@param r2 Ring 2 coordinate
@param r3 Ring 3 coordinate
@return Trajectory
Generate trajectory from startPos to ring 1 ring 2 ring 3 to endPos @param r1 Ring 1 coordinate @param r2 Ring 2 coordinate @param r3 Ring 3 coordinate @return Trajectory
ArrayList<Trajectory> pickUpAndShoot(Vector2d r1, Vector2d r2, Vector2d r3) {
        Logger.logFile("pickUpAndShoot " + r1 + "," + r2 + ", " + r3);
        currBuilder = null;
        addMove(startPose, r1, r2);
        addMove(new Pose2d(r1.getX(), r1.getY(), lastHeading), r2, r3);
        addMove(new Pose2d(r2.getX(), r2.getY(), lastHeading), r3, endPose.vec());
        currBuilder.splineToSplineHeading(endPose, endPose.getHeading());
        moves.add(currBuilder.build());
        return moves;
    }
ArrayList < Trajectory > pickUpAndShoot ( Vector2d r1 , Vector2d r2 , Vector2d r3 ) { Logger . logFile ( "pickUpAndShoot " + r1 + "," + r2 + ", " + r3 ) ; currBuilder = null ; addMove ( startPose , r1 , r2 ) ; addMove ( new Pose2d ( r1 . getX ( ) , r1 . getY ( ) , lastHeading ) , r2 , r3 ) ; addMove ( new Pose2d ( r2 . getX ( ) , r2 . getY ( ) , lastHeading ) , r3 , endPose . vec ( ) ) ; currBuilder . splineToSplineHeading ( endPose , endPose . getHeading ( ) ) ; moves . add ( currBuilder . build ( ) ) ; return moves ; }
Generate trajectory from startPos to ring 1, ring 2, ring 3 to endPos
 @param r1 Ring 1 coordinate
 @param r2 Ring 2 coordinate
 @param r3 Ring 3 coordinate
 @return Trajectory
Generate trajectory from startPos to ring 1 ring 2 ring 3 to endPos @param r1 Ring 1 coordinate @param r2 Ring 2 coordinate @param r3 Ring 3 coordinate @return Trajectory