FTCLib-Quickstart/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousOpMode.java at master · FTCLib/FTCLib-Quickstart · 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
package org.firstinspires.ftc.teamcode;

import com.arcrobotics.ftclib.command.CommandOpMode;
import com.arcrobotics.ftclib.command.RamseteCommand;
import com.arcrobotics.ftclib.controller.wpilibcontroller.RamseteController;
import com.arcrobotics.ftclib.geometry.Pose2d;
import com.arcrobotics.ftclib.geometry.Rotation2d;
import com.arcrobotics.ftclib.geometry.Translation2d;
import com.arcrobotics.ftclib.hardware.RevIMU;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.arcrobotics.ftclib.hardware.motors.MotorGroup;
import com.arcrobotics.ftclib.kinematics.wpilibkinematics.DifferentialDriveKinematics;
import com.arcrobotics.ftclib.trajectory.Trajectory;
import com.arcrobotics.ftclib.trajectory.TrajectoryConfig;
import com.arcrobotics.ftclib.trajectory.TrajectoryGenerator;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import org.firstinspires.ftc.teamcode.subsystems.DriveSubsystem;

import java.util.Arrays;

@Autonomous
public class AutonomousOpMode extends CommandOpMode {

    private Motor frontLeft, frontRight, backLeft, backRight;
    private RevIMU imu;
    private RamseteCommand ramseteFollower;
    private DriveSubsystem driveSubsystem;

    @Override
    public void initialize() {
        frontLeft = new Motor(hardwareMap, "front_left");
        frontRight = new Motor(hardwareMap, "front_right");
        backLeft = new Motor(hardwareMap, "back_left");
        backRight = new Motor(hardwareMap, "back_right");

        imu = new RevIMU(hardwareMap);
        imu.init();

        driveSubsystem = new DriveSubsystem(
                new MotorGroup(frontLeft, backLeft),
                new MotorGroup(frontRight, backRight),
                frontLeft.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE),
                frontRight.encoder.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE),
                imu,
                new Pose2d()
        );

        DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DriveConstants.TRACK_WIDTH);
        TrajectoryConfig config = new TrajectoryConfig(DriveConstants.MAX_VELOCITY, DriveConstants.MAX_ACCELERATION)
                .setKinematics(kinematics);

        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
                // Start at the origin facing the +X direction
                new Pose2d(0, 0, new Rotation2d(0)),
                // Pass through these two interior waypoints, making an 's' curve path
                Arrays.asList(new Translation2d(1, 1), new Translation2d(2, -1)),
                // End 3 meters straight ahead of where we started, facing forward
                new Pose2d(3, 0, new Rotation2d(0)),
                // Pass config
                config
        );

        ramseteFollower = new RamseteCommand(
                exampleTrajectory,
                driveSubsystem::getCurrentPose,
                new RamseteController(DriveConstants.B, DriveConstants.ZETA),
                kinematics,
                driveSubsystem::drive
        );

        schedule(ramseteFollower);
    }

}