@Autonomous
public class BlueFoundation extends LinearOpMode {
    MecanumBase drive = new MecanumAuto(hardwareMap);
    Foundation foundation = new Foundation();
    FtcDashboard dashboard = FtcDashboard.getInstance();
    Telemetry dashboardTelemetry = dashboard.getTelemetry();


    DriveConstraints slowConstraints = new DriveConstraints(25.0, 20.0, 0.0,
            Math.toRadians(180.0), Math.toRadians(180.0), 0.0);
    DriveConstraints fastConstraints = new DriveConstraints(40.0, 20.0, 0.0,
            Math.toRadians(180.0), Math.toRadians(180.0), 0.0);



    public void setup(){

    }
    public void runOpMode() throws InterruptedException{
        foundation.init(hardwareMap);
        Trajectory trajectory1 = new TrajectoryBuilder(new Pose2d(39.0, 63.0, Math.toRadians(180.0)), fastConstraints)
                .lineTo(new Vector2d(48.0, 33.0), new SplineInterpolator(Math.toRadians(90.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0, 33.0), ()->{
                    foundation.latchL.setPosition(1);//down
                    foundation.latchR.setPosition(1);//down

                    return Unit.INSTANCE;
                })
                .splineTo(new Pose2d(48.0, 35.0), new LinearInterpolator(Math.toRadians(90.0), Math.toRadians(90.0)))
                .addMarker(new Vector2d(48.0, 42.0), ()->{
                    foundation.latchL.setPosition(0);//up
                    foundation.latchR.setPosition(0);//up

                    return Unit.INSTANCE;
                })
                .lineTo(new Vector2d(0.0, 63.0), new SplineInterpolator(Math.toRadians(180.0), Math.toRadians(180.0)))
                .build();


        waitForStart();

        drive.followTrajectorySync(trajectory1);

    }
}