2866-Decode/SimpleAuto.java at main · Pattonville-Robotics/2866-Decode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
package org.firstinspires.ftc.teamcode;

import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.BezierLine;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;

@Autonomous(name = "Simple Auto (Pedro)")
public class SimpleAuto extends LinearOpMode {

    final boolean StopAtEnd = true;
    final boolean ContinueToNextPath = false;

    public static Follower follower;

    @Override
    public void runOpMode() {

        follower = Constants.createFollower(hardwareMap);

        //
        //  SET POSES HERE
        //

        Pose startPose = new Pose(72, 100, Math.toRadians(270)); // Start Pose of our robot.
        Pose shootPose = new Pose(72, 72, Math.toRadians(270));
        Pose endPose = new Pose(48, 72, Math.toRadians(270));

        //
        //  SET PATHS HERE
        //

        PathChain startToShoot = follower.pathBuilder()
                .addPath(new BezierLine(startPose, shootPose))
                .setLinearHeadingInterpolation(startPose.getHeading(), shootPose.getHeading())
                .build();

        PathChain shootToEnd = follower.pathBuilder()
                .addPath(new BezierLine(shootPose, endPose))
                .setLinearHeadingInterpolation(shootPose.getHeading(), endPose.getHeading())
                .build();

        PathChain endToStart = follower.pathBuilder()
                .addPath(new BezierLine(endPose, startPose))
                .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading())
                .build();

        //
        //  END OF PATHS
        //

        follower.setStartingPose(startPose);
        follower.setMaxPower(0.3);

        telemetry.addData("Status", "Initialized");
        telemetry.update();

        waitForStart();

        //
        //  FOLLOW PATHS HERE
        //

        follower.followPath(startToShoot, StopAtEnd);

        while (follower.isBusy() && opModeIsActive()) {
            follower.update();
        }

        // do something here like shoot
        sleep(5000);

        follower.followPath(shootToEnd, StopAtEnd);

        while (follower.isBusy() && opModeIsActive()) {
            follower.update();
        }

        follower.followPath(endToStart, StopAtEnd);

        while (follower.isBusy() && opModeIsActive()) {
            follower.update();
        }
    }
}