TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoVisionPickup.java
/**
     * We are at starting p1, need to add the move to p2 with p3 as next point
     */
We are at starting p1, need to add the move to p2 with p3 as next point
We are at starting p1 need to add the move to p2 with p3 as next point
void addMove(Pose2d p1, Vector2d p2, Vector2d p3) {
        Logger.logFile("Add Move " + p1 + " -> " + p2 + " -> " + p3);
        Line p1ToP2 = new Line(new Vector2D(p1.getX(), p1.getY()), new Vector2D(p2.getX(), p2.getY()), 0);
        Line p2ToP3 = new Line(new Vector2D(p2.getX(), p2.getY()), new Vector2D(p3.getX(), p3.getY()), 0 );
        boolean reverse = Math.abs((p1.getHeading() - p1ToP2.getAngle()))>Math.PI/2;  
        if (Math.abs(p1ToP2.getAngle() - p2ToP3.getAngle())>Math.PI) {
            lastHeading = (p2ToP3.getAngle() - p1ToP2.getAngle())/2 + Math.signum(p1ToP2.getAngle() - p2ToP3.getAngle()) * Math.PI;
        }
        else {
            lastHeading = (p1ToP2.getAngle() + p2ToP3.getAngle()) / 2;
        }
        if (currBuilder==null) {
            currBuilder = new TrajectoryBuilder(p1, reverse, constraints);
        }
        currBuilder.splineToSplineHeading(new Pose2d(p2.getX(), p2.getY(), lastHeading), lastHeading);
    }
void addMove ( Pose2d p1 , Vector2d p2 , Vector2d p3 ) { Logger . logFile ( "Add Move " + p1 + " -> " + p2 + " -> " + p3 ) ; Line p1ToP2 = new Line ( new Vector2D ( p1 . getX ( ) , p1 . getY ( ) ) , new Vector2D ( p2 . getX ( ) , p2 . getY ( ) ) , 0 ) ; Line p2ToP3 = new Line ( new Vector2D ( p2 . getX ( ) , p2 . getY ( ) ) , new Vector2D ( p3 . getX ( ) , p3 . getY ( ) ) , 0 ) ; boolean reverse = Math . abs ( ( p1 . getHeading ( ) - p1ToP2 . getAngle ( ) ) ) > Math . PI / 2 ; if ( Math . abs ( p1ToP2 . getAngle ( ) - p2ToP3 . getAngle ( ) ) > Math . PI ) { lastHeading = ( p2ToP3 . getAngle ( ) - p1ToP2 . getAngle ( ) ) / 2 + Math . signum ( p1ToP2 . getAngle ( ) - p2ToP3 . getAngle ( ) ) * Math . PI ; } else { lastHeading = ( p1ToP2 . getAngle ( ) + p2ToP3 . getAngle ( ) ) / 2 ; } if ( currBuilder == null ) { currBuilder = new TrajectoryBuilder ( p1 , reverse , constraints ) ; } currBuilder . splineToSplineHeading ( new Pose2d ( p2 . getX ( ) , p2 . getY ( ) , lastHeading ) , lastHeading ) ; }
We are at starting p1, need to add the move to p2 with p3 as next point
We are at starting p1 need to add the move to p2 with p3 as next point
// when the angle of more than 90 degree, reverse // The angle should try to pass P2 to P3