TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RingPickupPathGenerator.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);
        int w = isNearWall(p2);
        Vector2d p2f = null;
        if (w!=0) {
            p2 = new Vector2d(p2.getX()*whp[w-1].multiplierX + whp[w-1].entryOffsetX, p2.getY()*whp[w-1].multiplierY + whp[w-1].entryOffsetY);
            p2f = new Vector2d(p2.getX()*whp[w-1].multiplierX + whp[w-1].finalOffsetX, p2.getY()*whp[w-1].multiplierY + whp[w-1].finalOffsetY);
            lastHeading = whp[w-1].heading;
        }
        else {
            Line p2ToP3 = new Line(new Vector2D(p2.getX(), p2.getY()), new Vector2D(p3.getX(), p3.getY()), 0 );
            Line p2ToP1 = new Line(toVector2D(p2), toVector2D(p1), 0);
            double d1 = Math.min(p2.distTo(p1.vec()), ROBOT_WIDTH*2);
            double d2 = Math.min(p3.distTo(p2), ROBOT_WIDTH*2);
            double ang1 = getAngle(toVector2D(p2), toVector2D(p1));
            double ang2 = getAngle(toVector2D(p2), toVector2D(p3));
            if (Math.abs(ang1-ang2)<Math.PI/6) {
                lastHeading = getAngle(toVector2D(p1), toVector2D(p2));
                p2f = p2;
                p2 = new Vector2d(p2f.getX() - ROBOT_WIDTH/2 * Math.cos(lastHeading), p2f.getY() - ROBOT_WIDTH/2 * Math.sin(lastHeading));
                lastHeading = getAngle(toVector2D(p1), toVector2D(p2));
            }
            else {
                Vector2D tmpP1 = new Vector2D(p2.getX() + d2 * Math.cos(ang1), p2.getY() + d2 * Math.sin(ang1));
                Vector2D tmpP2 = new Vector2D(p2.getX() + d1 * Math.cos(ang2), p2.getY() + d1 * Math.sin(ang2));
                lastHeading = getAngle(tmpP1, tmpP2);
            }
        }
        if (currBuilder==null) {    
            Vector2D tmpP1a = new Vector2D(p1.getX() + Math.cos(p1.getHeading()), p1.getY() + Math.sin(p1.getHeading()));
            double d1p = tmpP1a.distance(toVector2D(p2));
            boolean reverse = d1p>p2.distTo(p1.vec());
            currBuilder = new TrajectoryBuilder(p1, reverse, constraints);
            Logger.logFile("ADS Spline from " + p1 + (reverse?" Reverse" : ""));
            if (reverse && p2f==null) {
                Pose2d preP2 = new Pose2d(p2.getX() - Math.cos(lastHeading) * ROBOT_WIDTH/2, p2.getY() - Math.sin(lastHeading)*ROBOT_WIDTH/2, lastHeading);
                currBuilder.splineToSplineHeading(preP2, lastHeading);
                Logger.logFile("ADS Spline to " + preP2 + " reverse");
            }
        }
        currBuilder.splineToSplineHeading(new Pose2d(p2, lastHeading), lastHeading);
        Logger.logFile("ADS Spline to " + p2);
        if (p2f != null) {
            currBuilder.splineToSplineHeading(new Pose2d(p2f, lastHeading), lastHeading, constraintsWall);
            moves.add(currBuilder.build());
            Logger.logFile("ADS Spline to " + p2f);
            currBuilder = new TrajectoryBuilder(new Pose2d(p2f, lastHeading), true, constraints);
            Logger.logFile("ADS Spline from " + p2f);
        }
    }
void addMove ( Pose2d p1 , Vector2d p2 , Vector2d p3 ) { Logger . logFile ( "Add Move " + p1 + " -> " + p2 + " -> " + p3 ) ; int w = isNearWall ( p2 ) ; Vector2d p2f = null ; if ( w != 0 ) { p2 = new Vector2d ( p2 . getX ( ) * whp [ w - 1 ] . multiplierX + whp [ w - 1 ] . entryOffsetX , p2 . getY ( ) * whp [ w - 1 ] . multiplierY + whp [ w - 1 ] . entryOffsetY ) ; p2f = new Vector2d ( p2 . getX ( ) * whp [ w - 1 ] . multiplierX + whp [ w - 1 ] . finalOffsetX , p2 . getY ( ) * whp [ w - 1 ] . multiplierY + whp [ w - 1 ] . finalOffsetY ) ; lastHeading = whp [ w - 1 ] . heading ; } else { Line p2ToP3 = new Line ( new Vector2D ( p2 . getX ( ) , p2 . getY ( ) ) , new Vector2D ( p3 . getX ( ) , p3 . getY ( ) ) , 0 ) ; Line p2ToP1 = new Line ( toVector2D ( p2 ) , toVector2D ( p1 ) , 0 ) ; double d1 = Math . min ( p2 . distTo ( p1 . vec ( ) ) , ROBOT_WIDTH * 2 ) ; double d2 = Math . min ( p3 . distTo ( p2 ) , ROBOT_WIDTH * 2 ) ; double ang1 = getAngle ( toVector2D ( p2 ) , toVector2D ( p1 ) ) ; double ang2 = getAngle ( toVector2D ( p2 ) , toVector2D ( p3 ) ) ; if ( Math . abs ( ang1 - ang2 ) < Math . PI / 6 ) { lastHeading = getAngle ( toVector2D ( p1 ) , toVector2D ( p2 ) ) ; p2f = p2 ; p2 = new Vector2d ( p2f . getX ( ) - ROBOT_WIDTH / 2 * Math . cos ( lastHeading ) , p2f . getY ( ) - ROBOT_WIDTH / 2 * Math . sin ( lastHeading ) ) ; lastHeading = getAngle ( toVector2D ( p1 ) , toVector2D ( p2 ) ) ; } else { Vector2D tmpP1 = new Vector2D ( p2 . getX ( ) + d2 * Math . cos ( ang1 ) , p2 . getY ( ) + d2 * Math . sin ( ang1 ) ) ; Vector2D tmpP2 = new Vector2D ( p2 . getX ( ) + d1 * Math . cos ( ang2 ) , p2 . getY ( ) + d1 * Math . sin ( ang2 ) ) ; lastHeading = getAngle ( tmpP1 , tmpP2 ) ; } } if ( currBuilder == null ) { Vector2D tmpP1a = new Vector2D ( p1 . getX ( ) + Math . cos ( p1 . getHeading ( ) ) , p1 . getY ( ) + Math . sin ( p1 . getHeading ( ) ) ) ; double d1p = tmpP1a . distance ( toVector2D ( p2 ) ) ; boolean reverse = d1p > p2 . distTo ( p1 . vec ( ) ) ; currBuilder = new TrajectoryBuilder ( p1 , reverse , constraints ) ; Logger . logFile ( "ADS Spline from " + p1 + ( reverse ? " Reverse" : "" ) ) ; if ( reverse && p2f == null ) { Pose2d preP2 = new Pose2d ( p2 . getX ( ) - Math . cos ( lastHeading ) * ROBOT_WIDTH / 2 , p2 . getY ( ) - Math . sin ( lastHeading ) * ROBOT_WIDTH / 2 , lastHeading ) ; currBuilder . splineToSplineHeading ( preP2 , lastHeading ) ; Logger . logFile ( "ADS Spline to " + preP2 + " reverse" ) ; } } currBuilder . splineToSplineHeading ( new Pose2d ( p2 , lastHeading ) , lastHeading ) ; Logger . logFile ( "ADS Spline to " + p2 ) ; if ( p2f != null ) { currBuilder . splineToSplineHeading ( new Pose2d ( p2f , lastHeading ) , lastHeading , constraintsWall ) ; moves . add ( currBuilder . build ( ) ) ; Logger . logFile ( "ADS Spline to " + p2f ) ; currBuilder = new TrajectoryBuilder ( new Pose2d ( p2f , lastHeading ) , true , constraints ) ; Logger . logFile ( "ADS Spline from " + p2f ) ; } }
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
// check if p2 is next to a wall // if near a wall, p2 will be the entrance position, p2f is the final position // if not near a wall, calculate the optimal robot heading to pick up ring based on the position of robot, // the ring and where it needs to get to next // special handling if robot comes in and need to back out with a sharp angle // then we should have the robot go straight pass the ring and do reverse to p3 // first move from starting position // Decide if we start robot in Reverse - if robot go on current heading for 1 inch, and become farther away from next point // Find the point 1 inch away with current heading // distance to next point // make sure it get the angle right if had to reverse on first leg, so we add a point before getting to the ring