getWheelVelocities() {
71 | // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
72 | // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
73 | // compensation method
74 |
75 | return Arrays.asList(
76 | encoderTicksToInches(leftEncoder.getRawVelocity()),
77 | encoderTicksToInches(rightEncoder.getRawVelocity()),
78 | encoderTicksToInches(frontEncoder.getRawVelocity())
79 | );
80 | }
81 | }
82 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.acmerobotics.roadrunner.trajectory.Trajectory;
6 | import com.arcrobotics.ftclib.command.CommandOpMode;
7 | import com.arcrobotics.ftclib.command.SequentialCommandGroup;
8 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
9 |
10 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
11 | import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
12 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
13 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
14 |
15 | /**
16 | * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
17 | * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
18 | * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
19 | * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
20 | * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
21 | * you've successfully connected, start the program, and your robot will begin moving forward and
22 | * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
23 | * your follower PID coefficients such that you follow the target position as accurately as possible.
24 | * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
25 | * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
26 | * These coefficients can be tuned live in dashboard.
27 | *
28 | * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
29 | * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
30 | *
31 | * NOTE: this has been refactored to use FTCLib's command-based
32 | */
33 | @Config
34 | @Autonomous(group = "drive")
35 | public class BackAndForth extends CommandOpMode {
36 |
37 | public static double DISTANCE = 50;
38 |
39 | private MecanumDriveSubsystem drive;
40 | private TrajectoryFollowerCommand forwardFollower, backwardFollower;
41 |
42 | @Override
43 | public void initialize() {
44 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
45 | Trajectory forwardTrajectory = drive.trajectoryBuilder(new Pose2d())
46 | .forward(DISTANCE)
47 | .build();
48 | forwardFollower = new TrajectoryFollowerCommand(drive, forwardTrajectory);
49 | backwardFollower = new TrajectoryFollowerCommand(drive,
50 | drive.trajectoryBuilder(forwardTrajectory.end())
51 | .back(DISTANCE)
52 | .build()
53 | );
54 | SequentialCommandGroup backAndForthCommand = new SequentialCommandGroup(forwardFollower, backwardFollower);
55 | schedule(new RunCommand(() -> {
56 | if (backAndForthCommand.isFinished() || !backAndForthCommand.isScheduled()) {
57 | backAndForthCommand.schedule();
58 | }
59 | }));
60 | }
61 |
62 | }
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FieldCentricTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.arcrobotics.ftclib.command.CommandOpMode;
5 | import com.arcrobotics.ftclib.gamepad.GamepadEx;
6 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
7 |
8 | import org.firstinspires.ftc.teamcode.commands.MecanumDriveCommand;
9 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
10 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
11 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
12 |
13 | /**
14 | * This is an additional test made specifically for FTCLib's quickstart.
15 | * It tests the field-centric mode of the {@link MecanumDriveSubsystem}.
16 | * The robot should always drive in the direction in which the left stick is
17 | * being pushed. This should be run after your {@link LocalizationTest}
18 | * to ensure proper tuning for the localization's heading estimate.
19 | *
20 | * @author Jackson
21 | */
22 | @Config
23 | @TeleOp
24 | public class FieldCentricTest extends CommandOpMode {
25 |
26 | private GamepadEx gamepad;
27 | private MecanumDriveSubsystem drive;
28 |
29 | @Override
30 | public void initialize() {
31 | gamepad = new GamepadEx(gamepad1);
32 |
33 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), true);
34 |
35 | register(drive);
36 | drive.setDefaultCommand(new MecanumDriveCommand(
37 | drive, () -> -gamepad.getLeftY(), gamepad::getLeftX, gamepad::getRightX
38 | ));
39 |
40 | schedule(new RunCommand(() -> {
41 | drive.update();
42 | telemetry.addData("Heading", drive.getPoseEstimate().getHeading());
43 | telemetry.update();
44 | }));
45 | }
46 |
47 | }
48 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.acmerobotics.roadrunner.trajectory.Trajectory;
6 | import com.arcrobotics.ftclib.command.Command;
7 | import com.arcrobotics.ftclib.command.CommandOpMode;
8 | import com.arcrobotics.ftclib.command.SequentialCommandGroup;
9 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
10 |
11 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
12 | import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
13 | import org.firstinspires.ftc.teamcode.commands.TurnCommand;
14 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
15 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
16 |
17 | /**
18 | * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
19 | * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
20 | * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
21 | * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
22 | * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
23 | * you've successfully connected, start the program, and your robot will begin driving in a square.
24 | * You should observe the target position (green) and your pose estimate (blue) and adjust your
25 | * follower PID coefficients such that you follow the target position as accurately as possible.
26 | * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
27 | * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
28 | * These coefficients can be tuned live in dashboard.
29 | *
30 | * NOTE: this has been refactored to use FTCLib's command-based
31 | */
32 | @Config
33 | @Autonomous(group = "drive")
34 | public class FollowerPIDTuner extends CommandOpMode {
35 |
36 | public static double DISTANCE = 48; // in
37 |
38 | private MecanumDriveSubsystem drive;
39 | private TrajectoryFollowerCommand followerCommand;
40 | private Command trajGroup;
41 | private TurnCommand turnCommand;
42 | private Pose2d startPose;
43 |
44 | @Override
45 | public void initialize() {
46 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
47 |
48 | startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
49 |
50 | drive.setPoseEstimate(startPose);
51 |
52 | schedule(new RunCommand(() -> {
53 | if (trajGroup == null || trajGroup.isFinished() || !trajGroup.isScheduled()) {
54 | Trajectory traj = drive.trajectoryBuilder(startPose)
55 | .forward(DISTANCE)
56 | .build();
57 | followerCommand = new TrajectoryFollowerCommand(drive, traj);
58 | turnCommand = new TurnCommand(drive, Math.toRadians(90));
59 | trajGroup = new SequentialCommandGroup(followerCommand, turnCommand)
60 | .whenFinished(() -> startPose = traj.end().plus(new Pose2d(0, 0, Math.toRadians(90))));
61 | trajGroup.schedule();
62 | }
63 | }));
64 | }
65 |
66 | }
67 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.roadrunner.geometry.Pose2d;
4 | import com.arcrobotics.ftclib.command.CommandOpMode;
5 | import com.arcrobotics.ftclib.command.PerpetualCommand;
6 | import com.arcrobotics.ftclib.gamepad.GamepadEx;
7 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
8 |
9 | import org.firstinspires.ftc.teamcode.commands.MecanumDriveCommand;
10 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
11 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
12 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
13 |
14 | /**
15 | * This is a simple teleop routine for testing localization. Drive the robot around like a normal
16 | * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
17 | * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
18 | * exercise is to ascertain whether the localizer has been configured properly (note: the pure
19 | * encoder localizer heading may be significantly off if the track width has not been tuned).
20 | *
21 | * NOTE: this has been refactored to use FTCLib's command-based
22 | */
23 | @TeleOp(group = "drive")
24 | public class LocalizationTest extends CommandOpMode {
25 |
26 | private MecanumDriveSubsystem drive;
27 | private MecanumDriveCommand driveCommand;
28 | private GamepadEx gamepad;
29 |
30 | @Override
31 | public void initialize() {
32 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
33 |
34 | gamepad = new GamepadEx(gamepad1);
35 |
36 | schedule(new RunCommand(() -> {
37 | drive.update();
38 | Pose2d poseEstimate = drive.getPoseEstimate();
39 | telemetry.addData("x", poseEstimate.getX());
40 | telemetry.addData("y", poseEstimate.getY());
41 | telemetry.addData("heading", poseEstimate.getHeading());
42 | telemetry.update();
43 | }));
44 |
45 | driveCommand = new MecanumDriveCommand(
46 | drive, () -> -gamepad.getLeftY(),
47 | gamepad::getLeftX, gamepad::getRightX
48 | );
49 |
50 | schedule(driveCommand);
51 | }
52 |
53 | }
54 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.arcrobotics.ftclib.command.CommandOpMode;
6 | import com.arcrobotics.ftclib.command.InstantCommand;
7 | import com.arcrobotics.ftclib.command.WaitUntilCommand;
8 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
9 | import com.qualcomm.robotcore.hardware.VoltageSensor;
10 | import com.qualcomm.robotcore.util.ElapsedTime;
11 |
12 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
13 | import org.firstinspires.ftc.teamcode.drive.DriveConstants;
14 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
15 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
16 |
17 | import java.util.Objects;
18 |
19 | /**
20 | * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
21 | * will also calculate the effective kF value for your velocity PID.
22 | *
23 | * Upon pressing start, your bot will run at max power for RUNTIME seconds.
24 | *
25 | * Further fine tuning of kF may be desired.
26 | *
27 | * NOTE: this has been refactored to use FTCLib's command-based
28 | */
29 | @Config
30 | @Autonomous(group = "drive")
31 | public class MaxVelocityTuner extends CommandOpMode {
32 |
33 | public static double RUNTIME = 2.0;
34 |
35 | private ElapsedTime timer = new ElapsedTime();
36 | private double maxVelocity = 0.0;
37 |
38 | private VoltageSensor batteryVoltageSensor;
39 |
40 | private MecanumDriveSubsystem drive;
41 |
42 | @Override
43 | public void initialize() {
44 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
45 | batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
46 |
47 | telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
48 | telemetry.addLine("Please ensure you have enough space cleared.");
49 | telemetry.addLine("");
50 | telemetry.addLine("Press start when ready.");
51 | telemetry.update();
52 |
53 | schedule(new WaitUntilCommand(this::isStarted)
54 | .whenFinished(() -> {
55 | telemetry.clearAll();
56 | telemetry.update();
57 |
58 | drive.setDrivePower(new Pose2d(1, 0, 0));
59 | timer.reset();
60 | })
61 | );
62 |
63 | RunCommand runCommand = new RunCommand(() -> {
64 | drive.update();
65 |
66 | Pose2d poseVelo = Objects.requireNonNull(
67 | drive.getPoseVelocity(),
68 | "poseVelocity() must not be null. " +
69 | "Ensure that the getWheelVelocities() method has been overridden in your localizer."
70 | );
71 |
72 | maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
73 | });
74 |
75 | schedule(new WaitUntilCommand(() -> timer.seconds() >= RUNTIME)
76 | .deadlineWith(runCommand)
77 | .whenFinished(() -> {
78 | drive.setDrivePower(new Pose2d());
79 |
80 | double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
81 |
82 | telemetry.addData("Max Velocity", maxVelocity);
83 | telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
84 | telemetry.update();
85 | })
86 | );
87 | }
88 |
89 | private double veloInchesToTicks(double inchesPerSec) {
90 | return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
91 | }
92 |
93 | }
94 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.roadrunner.geometry.Pose2d;
4 | import com.acmerobotics.roadrunner.geometry.Vector2d;
5 | import com.acmerobotics.roadrunner.trajectory.Trajectory;
6 | import com.arcrobotics.ftclib.command.CommandOpMode;
7 | import com.arcrobotics.ftclib.command.WaitCommand;
8 | import com.arcrobotics.ftclib.command.WaitUntilCommand;
9 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
10 |
11 | import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
12 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
13 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
14 |
15 | /**
16 | * This is an example of a more complex path to really test the tuning.
17 | *
18 | * NOTE: this has been refactored to use FTCLib's command-based
19 | */
20 | @Autonomous(group = "drive")
21 | public class SplineTest extends CommandOpMode {
22 |
23 | private MecanumDriveSubsystem drive;
24 | private TrajectoryFollowerCommand splineFollower;
25 |
26 | @Override
27 | public void initialize() {
28 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
29 | Trajectory traj = drive.trajectoryBuilder(new Pose2d())
30 | .splineTo(new Vector2d(30, 30), 0)
31 | .build();
32 | splineFollower = new TrajectoryFollowerCommand(drive, traj);
33 | schedule(new WaitUntilCommand(this::isStarted).andThen(
34 | splineFollower.andThen(new WaitCommand(2000),
35 | new TrajectoryFollowerCommand(drive,
36 | drive.trajectoryBuilder(traj.end(), true)
37 | .splineTo(new Vector2d(0, 0), Math.toRadians(180))
38 | .build()
39 | ))
40 | ));
41 | }
42 |
43 | }
44 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.arcrobotics.ftclib.command.CommandOpMode;
6 | import com.arcrobotics.ftclib.command.WaitUntilCommand;
7 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
8 |
9 | import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
10 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
11 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
12 |
13 | /**
14 | * This is a simple routine to test translational drive capabilities.
15 | *
16 | * NOTE: this has been refactored to use FTCLib's command-based
17 | */
18 | @Config
19 | @Autonomous(group = "drive")
20 | public class StrafeTest extends CommandOpMode {
21 |
22 | public static double DISTANCE = 60; // in
23 |
24 | private MecanumDriveSubsystem drive;
25 | private TrajectoryFollowerCommand strafeFollower;
26 |
27 | @Override
28 | public void initialize() {
29 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
30 | strafeFollower = new TrajectoryFollowerCommand(drive,
31 | drive.trajectoryBuilder(new Pose2d())
32 | .strafeRight(DISTANCE)
33 | .build()
34 | );
35 | schedule(new WaitUntilCommand(this::isStarted).andThen(strafeFollower.whenFinished(() -> {
36 | Pose2d poseEstimate = drive.getPoseEstimate();
37 | telemetry.addData("finalX", poseEstimate.getX());
38 | telemetry.addData("finalY", poseEstimate.getY());
39 | telemetry.addData("finalHeading", poseEstimate.getHeading());
40 | telemetry.update();
41 | })));
42 | }
43 |
44 | }
45 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.arcrobotics.ftclib.command.CommandOpMode;
6 | import com.arcrobotics.ftclib.command.WaitUntilCommand;
7 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
8 |
9 | import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
10 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
11 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
12 |
13 | /**
14 | * This is a simple routine to test translational drive capabilities.
15 | *
16 | * NOTE: this has been refactored to use FTCLib's command-based
17 | */
18 | @Config
19 | @Autonomous(group = "drive")
20 | public class StraightTest extends CommandOpMode {
21 |
22 | public static double DISTANCE = 60; // in
23 |
24 | private MecanumDriveSubsystem drive;
25 | private TrajectoryFollowerCommand straightFollower;
26 |
27 | @Override
28 | public void initialize() {
29 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
30 | straightFollower = new TrajectoryFollowerCommand(drive,
31 | drive.trajectoryBuilder(new Pose2d())
32 | .forward(DISTANCE)
33 | .build()
34 | );
35 | schedule(new WaitUntilCommand(this::isStarted).andThen(straightFollower.whenFinished(() -> {
36 | Pose2d poseEstimate = drive.getPoseEstimate();
37 | telemetry.addData("finalX", poseEstimate.getX());
38 | telemetry.addData("finalY", poseEstimate.getY());
39 | telemetry.addData("finalHeading", poseEstimate.getHeading());
40 | telemetry.update();
41 | })));
42 | }
43 |
44 | }
45 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.FtcDashboard;
4 | import com.acmerobotics.dashboard.config.Config;
5 | import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
6 | import com.acmerobotics.roadrunner.geometry.Pose2d;
7 | import com.acmerobotics.roadrunner.util.Angle;
8 | import com.arcrobotics.ftclib.command.CommandOpMode;
9 | import com.arcrobotics.ftclib.command.InstantCommand;
10 | import com.arcrobotics.ftclib.command.SequentialCommandGroup;
11 | import com.arcrobotics.ftclib.command.WaitUntilCommand;
12 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
13 | import com.qualcomm.robotcore.util.MovingStatistics;
14 |
15 | import org.firstinspires.ftc.robotcore.internal.system.Misc;
16 | import org.firstinspires.ftc.teamcode.commands.RunCommand;
17 | import org.firstinspires.ftc.teamcode.commands.TurnCommand;
18 | import org.firstinspires.ftc.teamcode.drive.DriveConstants;
19 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
20 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
21 |
22 | /**
23 | * This routine determines the effective track width. The procedure works by executing a point turn
24 | * with a given angle and measuring the difference between that angle and the actual angle (as
25 | * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
26 | * given angle / actual angle gives a multiplicative adjustment to the estimated track width
27 | * (effective track width = estimated track width * given angle / actual angle). The routine repeats
28 | * this procedure a few times and averages the values for additional accuracy. Note: a relatively
29 | * accurate track width estimate is important or else the angular constraints will be thrown off.
30 | *
31 | * NOTE: this has been refactored to use FTCLib's command-based
32 | */
33 | @Config
34 | @Autonomous(group = "drive")
35 | public class TrackWidthTuner extends CommandOpMode {
36 |
37 | public static double ANGLE = 180; // deg
38 | public static int NUM_TRIALS = 5;
39 | public static int DELAY = 1000; // ms
40 |
41 | private MecanumDriveSubsystem drive;
42 | private MovingStatistics trackWidthStats;
43 | private TurnCommand turnCommand;
44 | private double headingAccumulator, lastHeading;
45 | private int trial;
46 |
47 | @Override
48 | public void initialize() {
49 | telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
50 |
51 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
52 | // TODO: if you haven't already, set the localizer to something that doesn't depend on
53 | // drive encoders for computing the heading
54 |
55 | turnCommand = new TurnCommand(drive, Math.toRadians(ANGLE));
56 |
57 | telemetry.addLine("Press play to begin the track width tuner routine");
58 | telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
59 | telemetry.update();
60 |
61 | SequentialCommandGroup setupCommand = new SequentialCommandGroup(
62 | new WaitUntilCommand(this::isStarted),
63 | new InstantCommand(() -> {
64 | telemetry.clearAll();
65 | telemetry.addLine("Running...");
66 | telemetry.update();
67 |
68 | trackWidthStats = new MovingStatistics(NUM_TRIALS);
69 | trial = 0;
70 | }
71 | ));
72 |
73 | InstantCommand finishCommand = new InstantCommand(() -> {
74 | telemetry.clearAll();
75 | telemetry.addLine("Tuning complete");
76 | telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
77 | trackWidthStats.getMean(),
78 | trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
79 | telemetry.update();
80 | });
81 |
82 | RunCommand tuneCommand = new RunCommand(() -> {
83 | if (trial < NUM_TRIALS && (turnCommand == null || !turnCommand.isScheduled())) {
84 | if (headingAccumulator != 0) {
85 | double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
86 | trackWidthStats.add(trackWidth);
87 | }
88 |
89 | sleep(DELAY);
90 |
91 | trial++;
92 |
93 | drive.setPoseEstimate(new Pose2d());
94 |
95 | // it is important to handle heading wraparounds
96 | headingAccumulator = 0;
97 | lastHeading = 0;
98 |
99 | turnCommand = new TurnCommand(drive, Math.toRadians(ANGLE));
100 | turnCommand.schedule();
101 | } else {
102 | double heading = drive.getPoseEstimate().getHeading();
103 | headingAccumulator += Angle.norm(heading - lastHeading);
104 | lastHeading = heading;
105 | }
106 | });
107 |
108 | schedule(setupCommand.andThen(
109 | new WaitUntilCommand(() -> trial == NUM_TRIALS)
110 | .deadlineWith(tuneCommand)
111 | .whenFinished(turnCommand::cancel),
112 | finishCommand
113 | ));
114 | }
115 |
116 | }
117 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.drive.opmode;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.arcrobotics.ftclib.command.CommandOpMode;
6 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
7 |
8 | import org.firstinspires.ftc.teamcode.commands.TurnCommand;
9 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
10 | import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
11 |
12 | /**
13 | * This is a simple routine to test turning capabilities.
14 | *
15 | * NOTE: this has been refactored to use FTCLib's command-based
16 | */
17 | @Config
18 | @Autonomous(group = "drive")
19 | public class TurnTest extends CommandOpMode {
20 |
21 | public static double ANGLE = 90; // deg
22 |
23 | private MecanumDriveSubsystem drive;
24 | private TurnCommand turnCommand;
25 |
26 | @Override
27 | public void initialize() {
28 | drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), false);
29 | turnCommand = new TurnCommand(drive, Math.toRadians(ANGLE));
30 | schedule(turnCommand.whenFinished(() -> {
31 | Pose2d poseEstimate = drive.getPoseEstimate();
32 | telemetry.addData("finalX", poseEstimate.getX());
33 | telemetry.addData("finalY", poseEstimate.getY());
34 | telemetry.addData("finalHeading", poseEstimate.getHeading());
35 | telemetry.update();
36 | }));
37 | }
38 |
39 | }
40 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.subsystems;
2 |
3 | import com.acmerobotics.roadrunner.geometry.Pose2d;
4 | import com.acmerobotics.roadrunner.geometry.Vector2d;
5 | import com.acmerobotics.roadrunner.localization.Localizer;
6 | import com.acmerobotics.roadrunner.trajectory.Trajectory;
7 | import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
8 | import com.arcrobotics.ftclib.command.SubsystemBase;
9 | import com.qualcomm.robotcore.hardware.DcMotor;
10 | import com.qualcomm.robotcore.hardware.PIDFCoefficients;
11 |
12 | import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
13 |
14 | import java.util.List;
15 |
16 | /**
17 | * A subsystem that uses the {@link SampleMecanumDrive} class.
18 | * This periodically calls {@link SampleMecanumDrive#update()} which runs the internal
19 | * state machine for the mecanum drive. All movement/following is async to fit the paradigm.
20 | */
21 | public class MecanumDriveSubsystem extends SubsystemBase {
22 |
23 | private final SampleMecanumDrive drive;
24 | private final boolean fieldCentric;
25 |
26 | public MecanumDriveSubsystem(SampleMecanumDrive drive, boolean isFieldCentric) {
27 | this.drive = drive;
28 | fieldCentric = isFieldCentric;
29 | }
30 |
31 | public void setMode(DcMotor.RunMode mode) {
32 | drive.setMode(mode);
33 | }
34 |
35 | public void setPIDFCoefficients(DcMotor.RunMode mode, PIDFCoefficients coefficients) {
36 | drive.setPIDFCoefficients(mode, coefficients);
37 | }
38 |
39 | public void setPoseEstimate(Pose2d pose) {
40 | drive.setPoseEstimate(pose);
41 | }
42 |
43 | public void update() {
44 | drive.update();
45 | }
46 |
47 | public void updatePoseEstimate() {
48 | drive.updatePoseEstimate();
49 | }
50 |
51 | public void drive(double leftY, double leftX, double rightX) {
52 | Pose2d poseEstimate = getPoseEstimate();
53 |
54 | Vector2d input = new Vector2d(-leftY, -leftX).rotated(
55 | fieldCentric ? -poseEstimate.getHeading() : 0
56 | );
57 |
58 | drive.setWeightedDrivePower(
59 | new Pose2d(
60 | input.getX(),
61 | input.getY(),
62 | -rightX
63 | )
64 | );
65 | }
66 |
67 | public void setDrivePower(Pose2d drivePower) {
68 | drive.setDrivePower(drivePower);
69 | }
70 |
71 | public Pose2d getPoseEstimate() {
72 | return drive.getPoseEstimate();
73 | }
74 |
75 | public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
76 | return drive.trajectoryBuilder(startPose);
77 | }
78 |
79 | public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
80 | return drive.trajectoryBuilder(startPose, reversed);
81 | }
82 |
83 | public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
84 | return drive.trajectoryBuilder(startPose, startHeading);
85 | }
86 |
87 | public void followTrajectory(Trajectory trajectory) {
88 | drive.followTrajectoryAsync(trajectory);
89 | }
90 |
91 | public boolean isBusy() {
92 | return drive.isBusy();
93 | }
94 |
95 | public void turn(double radians) {
96 | drive.turnAsync(radians);
97 | }
98 |
99 | public List getWheelVelocities() {
100 | return drive.getWheelVelocities();
101 | }
102 |
103 | public void stop() {
104 | drive(0, 0, 0);
105 | }
106 |
107 | public Pose2d getPoseVelocity() {
108 | return drive.getPoseVelocity();
109 | }
110 |
111 | public Localizer getLocalizer() {
112 | return drive.getLocalizer();
113 | }
114 |
115 | }
116 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import androidx.annotation.Nullable;
4 |
5 | import com.acmerobotics.roadrunner.trajectory.Trajectory;
6 | import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
7 | import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
8 | import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
9 | import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
10 |
11 | import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
12 |
13 | import java.io.IOException;
14 | import java.io.InputStream;
15 |
16 | /**
17 | * Set of utilities for loading trajectories from assets (the plugin save location).
18 | */
19 | public class AssetsTrajectoryManager {
20 |
21 | /**
22 | * Loads the group config.
23 | */
24 | public static @Nullable
25 | TrajectoryGroupConfig loadGroupConfig() {
26 | try {
27 | InputStream inputStream = AppUtil.getDefContext().getAssets().open(
28 | "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
29 | return TrajectoryConfigManager.loadGroupConfig(inputStream);
30 | } catch (IOException e) {
31 | return null;
32 | }
33 | }
34 |
35 | /**
36 | * Loads a trajectory config with the given name.
37 | */
38 | public static @Nullable TrajectoryConfig loadConfig(String name) {
39 | try {
40 | InputStream inputStream = AppUtil.getDefContext().getAssets().open(
41 | "trajectory/" + name + ".yaml");
42 | return TrajectoryConfigManager.loadConfig(inputStream);
43 | } catch (IOException e) {
44 | return null;
45 | }
46 | }
47 |
48 | /**
49 | * Loads a trajectory builder with the given name.
50 | */
51 | public static @Nullable TrajectoryBuilder loadBuilder(String name) {
52 | TrajectoryGroupConfig groupConfig = loadGroupConfig();
53 | TrajectoryConfig config = loadConfig(name);
54 | if (groupConfig == null || config == null) {
55 | return null;
56 | }
57 | return config.toTrajectoryBuilder(groupConfig);
58 | }
59 |
60 | /**
61 | * Loads a trajectory with the given name.
62 | */
63 | public static @Nullable Trajectory load(String name) {
64 | TrajectoryBuilder builder = loadBuilder(name);
65 | if (builder == null) {
66 | return null;
67 | }
68 | return builder.build();
69 | }
70 | }
71 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | /**
4 | * IMU axes signs in the order XYZ (after remapping).
5 | */
6 | public enum AxesSigns {
7 | PPP(0b000),
8 | PPN(0b001),
9 | PNP(0b010),
10 | PNN(0b011),
11 | NPP(0b100),
12 | NPN(0b101),
13 | NNP(0b110),
14 | NNN(0b111);
15 |
16 | public final int bVal;
17 |
18 | AxesSigns(int bVal) {
19 | this.bVal = bVal;
20 | }
21 | }
22 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055IMUUtil.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.qualcomm.hardware.bosch.BNO055IMU;
4 |
5 | import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
6 |
7 | /**
8 | * Various utility functions for the BNO055 IMU.
9 | */
10 | public class BNO055IMUUtil {
11 | /**
12 | * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#ZYX}.
13 | * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
14 | * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
15 | *
16 | * Adapted from this post.
17 | *
18 | * @param imu IMU
19 | * @param order axes order
20 | * @param signs axes signs
21 | */
22 | public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
23 | try {
24 | // the indices correspond with the 2-bit encodings specified in the datasheet
25 | int[] indices = order.indices();
26 | int axisMapConfig = 0;
27 | axisMapConfig |= (indices[0] << 4);
28 | axisMapConfig |= (indices[1] << 2);
29 | axisMapConfig |= (indices[2] << 0);
30 |
31 | // the BNO055 driver flips the first orientation vector so we also flip here
32 | int axisMapSign = signs.bVal ^ (0b100 >> indices[0]);
33 |
34 | // Enter CONFIG mode
35 | imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
36 |
37 | Thread.sleep(100);
38 |
39 | // Write the AXIS_MAP_CONFIG register
40 | imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
41 |
42 | // Write the AXIS_MAP_SIGN register
43 | imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign & 0x07);
44 |
45 | // Switch back to the previous mode
46 | imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
47 |
48 | Thread.sleep(100);
49 | } catch (InterruptedException e) {
50 | Thread.currentThread().interrupt();
51 | }
52 | }
53 | }
54 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.acmerobotics.dashboard.canvas.Canvas;
4 | import com.acmerobotics.roadrunner.geometry.Pose2d;
5 | import com.acmerobotics.roadrunner.geometry.Vector2d;
6 | import com.acmerobotics.roadrunner.path.Path;
7 |
8 | import java.util.List;
9 |
10 | /**
11 | * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
12 | */
13 | public class DashboardUtil {
14 | private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
15 | private static final double ROBOT_RADIUS = 9; // in
16 |
17 |
18 | public static void drawPoseHistory(Canvas canvas, List poseHistory) {
19 | double[] xPoints = new double[poseHistory.size()];
20 | double[] yPoints = new double[poseHistory.size()];
21 | for (int i = 0; i < poseHistory.size(); i++) {
22 | Pose2d pose = poseHistory.get(i);
23 | xPoints[i] = pose.getX();
24 | yPoints[i] = pose.getY();
25 | }
26 | canvas.strokePolyline(xPoints, yPoints);
27 | }
28 |
29 | public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
30 | int samples = (int) Math.ceil(path.length() / resolution);
31 | double[] xPoints = new double[samples];
32 | double[] yPoints = new double[samples];
33 | double dx = path.length() / (samples - 1);
34 | for (int i = 0; i < samples; i++) {
35 | double displacement = i * dx;
36 | Pose2d pose = path.get(displacement);
37 | xPoints[i] = pose.getX();
38 | yPoints[i] = pose.getY();
39 | }
40 | canvas.strokePolyline(xPoints, yPoints);
41 | }
42 |
43 | public static void drawSampledPath(Canvas canvas, Path path) {
44 | drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
45 | }
46 |
47 | public static void drawRobot(Canvas canvas, Pose2d pose) {
48 | canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
49 | Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
50 | double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
51 | double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
52 | canvas.strokeLine(x1, y1, x2, y2);
53 | }
54 | }
55 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.acmerobotics.roadrunner.util.NanoClock;
4 | import com.qualcomm.robotcore.hardware.DcMotorEx;
5 |
6 | /**
7 | * Wraps a motor instance to provide corrected velocity counts and allow reversing without changing the corresponding
8 | * slot's motor direction
9 | */
10 | public class Encoder {
11 | private final static int CPS_STEP = 0x10000;
12 |
13 | private static double inverseOverflow(double input, double estimate) {
14 | double real = input;
15 | while (Math.abs(estimate - real) > CPS_STEP / 2.0) {
16 | real += Math.signum(estimate - real) * CPS_STEP;
17 | }
18 | return real;
19 | }
20 |
21 | public enum Direction {
22 | FORWARD(1),
23 | REVERSE(-1);
24 |
25 | private int multiplier;
26 |
27 | Direction(int multiplier) {
28 | this.multiplier = multiplier;
29 | }
30 |
31 | public int getMultiplier() {
32 | return multiplier;
33 | }
34 | }
35 |
36 | private DcMotorEx motor;
37 | private NanoClock clock;
38 |
39 | private Direction direction;
40 |
41 | private int lastPosition;
42 | private double velocityEstimate;
43 | private double lastUpdateTime;
44 |
45 | public Encoder(DcMotorEx motor, NanoClock clock) {
46 | this.motor = motor;
47 | this.clock = clock;
48 |
49 | this.direction = Direction.FORWARD;
50 |
51 | this.lastPosition = 0;
52 | this.velocityEstimate = 0.0;
53 | this.lastUpdateTime = clock.seconds();
54 | }
55 |
56 | public Encoder(DcMotorEx motor) {
57 | this(motor, NanoClock.system());
58 | }
59 |
60 | public Direction getDirection() {
61 | return direction;
62 | }
63 |
64 | /**
65 | * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
66 | * @param direction either reverse or forward depending on if encoder counts should be negated
67 | */
68 | public void setDirection(Direction direction) {
69 | this.direction = direction;
70 | }
71 |
72 | public int getCurrentPosition() {
73 | int multiplier = direction.getMultiplier();
74 | int currentPosition = motor.getCurrentPosition() * multiplier;
75 | if (currentPosition != lastPosition) {
76 | double currentTime = clock.seconds();
77 | double dt = currentTime - lastUpdateTime;
78 | velocityEstimate = (currentPosition - lastPosition) / dt;
79 | lastPosition = currentPosition;
80 | lastUpdateTime = currentTime;
81 | }
82 | return currentPosition;
83 | }
84 |
85 | public double getRawVelocity() {
86 | int multiplier = direction.getMultiplier();
87 | return motor.getVelocity() * multiplier;
88 | }
89 |
90 | public double getCorrectedVelocity() {
91 | return inverseOverflow(getRawVelocity(), velocityEstimate);
92 | }
93 | }
94 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
4 |
5 | import java.io.File;
6 | import java.util.ArrayList;
7 | import java.util.Collections;
8 | import java.util.List;
9 |
10 | /**
11 | * Utility functions for log files.
12 | */
13 | public class LoggingUtil {
14 | public static final File ROAD_RUNNER_FOLDER =
15 | new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
16 |
17 | private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
18 |
19 | private static void buildLogList(List logFiles, File dir) {
20 | for (File file : dir.listFiles()) {
21 | if (file.isDirectory()) {
22 | buildLogList(logFiles, file);
23 | } else {
24 | logFiles.add(file);
25 | }
26 | }
27 | }
28 |
29 | private static void pruneLogsIfNecessary() {
30 | List logFiles = new ArrayList<>();
31 | buildLogList(logFiles, ROAD_RUNNER_FOLDER);
32 | Collections.sort(logFiles, (lhs, rhs) ->
33 | Long.compare(lhs.lastModified(), rhs.lastModified()));
34 |
35 | long dirSize = 0;
36 | for (File file: logFiles) {
37 | dirSize += file.length();
38 | }
39 |
40 | while (dirSize > LOG_QUOTA) {
41 | if (logFiles.size() == 0) break;
42 | File fileToRemove = logFiles.remove(0);
43 | dirSize -= fileToRemove.length();
44 | //noinspection ResultOfMethodCallIgnored
45 | fileToRemove.delete();
46 | }
47 | }
48 |
49 | /**
50 | * Obtain a log file with the provided name
51 | */
52 | public static File getLogFile(String name) {
53 | //noinspection ResultOfMethodCallIgnored
54 | ROAD_RUNNER_FOLDER.mkdirs();
55 |
56 | pruneLogsIfNecessary();
57 |
58 | return new File(ROAD_RUNNER_FOLDER, name);
59 | }
60 | }
61 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.qualcomm.hardware.lynx.LynxModule;
4 | import com.qualcomm.robotcore.hardware.HardwareMap;
5 |
6 | import org.firstinspires.ftc.robotcore.internal.system.Misc;
7 |
8 | import java.util.HashMap;
9 | import java.util.Map;
10 |
11 | /**
12 | * Collection of utilites for interacting with Lynx modules.
13 | */
14 | public class LynxModuleUtil {
15 |
16 | private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
17 |
18 | /**
19 | * Parsed representation of a Lynx module firmware version.
20 | */
21 | public static class LynxFirmwareVersion implements Comparable {
22 | public final int major;
23 | public final int minor;
24 | public final int eng;
25 |
26 | public LynxFirmwareVersion(int major, int minor, int eng) {
27 | this.major = major;
28 | this.minor = minor;
29 | this.eng = eng;
30 | }
31 |
32 | @Override
33 | public boolean equals(Object other) {
34 | if (other instanceof LynxFirmwareVersion) {
35 | LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
36 | return major == otherVersion.major && minor == otherVersion.minor &&
37 | eng == otherVersion.eng;
38 | } else {
39 | return false;
40 | }
41 | }
42 |
43 | @Override
44 | public int compareTo(LynxFirmwareVersion other) {
45 | int majorComp = Integer.compare(major, other.major);
46 | if (majorComp == 0) {
47 | int minorComp = Integer.compare(minor, other.minor);
48 | if (minorComp == 0) {
49 | return Integer.compare(eng, other.eng);
50 | } else {
51 | return minorComp;
52 | }
53 | } else {
54 | return majorComp;
55 | }
56 | }
57 |
58 | @Override
59 | public String toString() {
60 | return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
61 | }
62 | }
63 |
64 | /**
65 | * Retrieve and parse Lynx module firmware version.
66 | * @param module Lynx module
67 | * @return parsed firmware version
68 | */
69 | public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
70 | String versionString = module.getNullableFirmwareVersionString();
71 | if (versionString == null) {
72 | return null;
73 | }
74 |
75 | String[] parts = versionString.split("[ :,]+");
76 | try {
77 | // note: for now, we ignore the hardware entry
78 | return new LynxFirmwareVersion(
79 | Integer.parseInt(parts[3]),
80 | Integer.parseInt(parts[5]),
81 | Integer.parseInt(parts[7])
82 | );
83 | } catch (NumberFormatException e) {
84 | return null;
85 | }
86 | }
87 |
88 | /**
89 | * Exception indicating an outdated Lynx firmware version.
90 | */
91 | public static class LynxFirmwareVersionException extends RuntimeException {
92 | public LynxFirmwareVersionException(String detailMessage) {
93 | super(detailMessage);
94 | }
95 | }
96 |
97 | /**
98 | * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
99 | * @param hardwareMap hardware map containing Lynx modules
100 | */
101 | public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
102 | Map outdatedModules = new HashMap<>();
103 | for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
104 | LynxFirmwareVersion version = getFirmwareVersion(module);
105 | if (version == null || version.compareTo(MIN_VERSION) < 0) {
106 | for (String name : hardwareMap.getNamesOf(module)) {
107 | outdatedModules.put(name, version);
108 | }
109 | }
110 | }
111 | if (outdatedModules.size() > 0) {
112 | StringBuilder msgBuilder = new StringBuilder();
113 | msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
114 | msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version: %s\n",
115 | MIN_VERSION.toString()));
116 | for (Map.Entry entry : outdatedModules.entrySet()) {
117 | msgBuilder.append(Misc.formatInvariant(
118 | "\t%s: %s\n", entry.getKey(),
119 | entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
120 | }
121 | throw new LynxFirmwareVersionException(msgBuilder.toString());
122 | }
123 | }
124 | }
125 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Pose2dUtil.kt:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util
2 |
3 | import com.acmerobotics.roadrunner.geometry.Pose2d
4 | import com.arcrobotics.ftclib.geometry.Rotation2d
5 | import com.arcrobotics.ftclib.kinematics.wpilibkinematics.ChassisSpeeds
6 |
7 | fun com.arcrobotics.ftclib.geometry.Pose2d.toRR(): Pose2d = Pose2d(this.translation.y, this.translation.x, this.heading)
8 |
9 | fun Pose2d.toFTCLib(): com.arcrobotics.ftclib.geometry.Pose2d = com.arcrobotics.ftclib.geometry.Pose2d(-this.x, -this.y, Rotation2d(this.heading))
10 |
11 | val com.arcrobotics.ftclib.geometry.Pose2d.x: Double
12 | get() = this.translation.x
13 |
14 | val com.arcrobotics.ftclib.geometry.Pose2d.y: Double
15 | get() = this.translation.y
16 |
17 | val com.arcrobotics.ftclib.geometry.Pose2d.rotationDeg: Double
18 | get() = this.rotation.degrees
19 |
20 | val com.arcrobotics.ftclib.geometry.Pose2d.rotationRad: Double
21 | get() = this.rotation.radians
22 |
23 | fun ChassisSpeeds.toRRPose2d(): Pose2d = Pose2d(this.vxMetersPerSecond, this.vyMetersPerSecond, this.omegaRadiansPerSecond)
--------------------------------------------------------------------------------
/TeamCode/src/main/res/raw/readme.md:
--------------------------------------------------------------------------------
1 | Place your sound files in this folder to use them as project resources.
--------------------------------------------------------------------------------
/TeamCode/src/main/res/values/strings.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
--------------------------------------------------------------------------------
/build.common.gradle:
--------------------------------------------------------------------------------
1 | /**
2 | * build.common.gradle
3 | *
4 | * Try to avoid editing this file, as it may be updated from time to time as the FTC SDK
5 | * evolves. Rather, if it is necessary to customize the build process, do those edits in
6 | * the build.gradle file in TeamCode.
7 | *
8 | * This file contains the necessary content of the 'build.gradle' files for robot controller
9 | * applications built using the FTC SDK. Each individual 'build.gradle' in those applications
10 | * can simply contain the one line:
11 | *
12 | * apply from: '../build.common.gradle'
13 | *
14 | * which will pick up this file here. This approach allows makes it easier to integrate
15 | * updates to the FTC SDK into your code.
16 | */
17 |
18 | import java.util.regex.Pattern
19 |
20 | apply plugin: 'com.android.application'
21 |
22 | def useProguard = false
23 |
24 | android {
25 |
26 | compileSdkVersion 29
27 |
28 | signingConfigs {
29 | debug {
30 | keyAlias 'androiddebugkey'
31 | keyPassword 'android'
32 | storeFile rootProject.file('libs/ftc.debug.keystore')
33 | storePassword 'android'
34 | }
35 | }
36 |
37 | aaptOptions {
38 | noCompress "tflite"
39 | }
40 |
41 | defaultConfig {
42 | signingConfig signingConfigs.debug
43 | applicationId 'com.qualcomm.ftcrobotcontroller'
44 | minSdkVersion 24
45 | targetSdkVersion 28
46 | multiDexEnabled !useProguard
47 |
48 | /**
49 | * We keep the versionCode and versionName of robot controller applications in sync with
50 | * the master information published in the AndroidManifest.xml file of the FtcRobotController
51 | * module. This helps avoid confusion that might arise from having multiple versions of
52 | * a robot controller app simultaneously installed on a robot controller device.
53 | *
54 | * We accomplish this with the help of a funky little Groovy script that maintains that
55 | * correspondence automatically.
56 | *
57 | * @see Configure Your Build
58 | * @see Versioning Your App
59 | */
60 | def manifestFile = project(':FtcRobotController').file('src/main/AndroidManifest.xml');
61 | def manifestText = manifestFile.getText()
62 | //
63 | def vCodePattern = Pattern.compile("versionCode=\"(\\d+(\\.\\d+)*)\"")
64 | def matcher = vCodePattern.matcher(manifestText)
65 | matcher.find()
66 | def vCode = Integer.parseInt(matcher.group(1))
67 | //
68 | def vNamePattern = Pattern.compile("versionName=\"(.*)\"")
69 | matcher = vNamePattern.matcher(manifestText);
70 | matcher.find()
71 | def vName = matcher.group(1)
72 | //
73 | versionCode vCode
74 | versionName vName
75 | }
76 |
77 | // Advanced user code might just want to use Vuforia directly, so we set up the libs as needed
78 | // http://google.github.io/android-gradle-dsl/current/com.android.build.gradle.internal.dsl.BuildType.html
79 | buildTypes {
80 | release {
81 | // Disable debugging for release versions so it can be uploaded to Google Play.
82 | //debuggable true
83 | ndk {
84 | abiFilters "armeabi-v7a", "arm64-v8a"
85 | }
86 |
87 | if (useProguard) {
88 | minifyEnabled true
89 | proguardFiles getDefaultProguardFile('proguard-android.txt'),
90 | 'proguard-rules.pro'
91 | println(getProguardFiles())
92 | }
93 | }
94 | debug {
95 | debuggable true
96 | jniDebuggable true
97 | renderscriptDebuggable true
98 | ndk {
99 | abiFilters "armeabi-v7a", "arm64-v8a"
100 | }
101 |
102 | if (useProguard) {
103 | minifyEnabled true
104 | proguardFiles getDefaultProguardFile('proguard-android.txt'),
105 | 'proguard-rules.pro'
106 | println(getProguardFiles())
107 | }
108 | }
109 | }
110 |
111 | compileOptions {
112 | sourceCompatibility JavaVersion.VERSION_1_8
113 | targetCompatibility JavaVersion.VERSION_1_8
114 | }
115 |
116 | sourceSets.main {
117 | jni.srcDirs = []
118 | jniLibs.srcDir rootProject.file('libs')
119 | }
120 | }
121 |
122 | repositories {
123 | flatDir {
124 | dirs rootProject.file('libs')
125 | }
126 | }
127 | apply from: 'build.release.gradle'
128 |
--------------------------------------------------------------------------------
/build.gradle:
--------------------------------------------------------------------------------
1 | /**
2 | * Top-level build file for ftc_app project.
3 | *
4 | * It is extraordinarily rare that you will ever need to edit this file.
5 | */
6 | buildscript {
7 | repositories {
8 | google()
9 | jcenter()
10 | }
11 | dependencies {
12 | classpath 'com.android.tools.build:gradle:4.0.1'
13 | }
14 | }
15 |
16 | // This is now required because aapt2 has to be downloaded from the
17 | // google() repository beginning with version 3.2 of the Android Gradle Plugin
18 | allprojects {
19 | repositories {
20 | google()
21 | jcenter()
22 | }
23 | }
24 |
--------------------------------------------------------------------------------
/doc/apk/FtcDriverStation-release.apk:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/apk/FtcDriverStation-release.apk
--------------------------------------------------------------------------------
/doc/apk/FtcRobotController-release.apk:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/apk/FtcRobotController-release.apk
--------------------------------------------------------------------------------
/doc/legal/AudioBlocksSounds.txt:
--------------------------------------------------------------------------------
1 | The sound files listed below in this SDK were purchased from www.audioblocks.com under the
2 | following license.
3 |
4 | http://support.audioblocks.com/customer/en/portal/topics/610636-licensing-faq-s/articles
5 |
6 | How am I allowed to use your content?
7 | Last Updated: Aug 11, 2016 01:51PM EDT
8 | Our content may be used for nearly any project, commercial or otherwise, including feature
9 | films, broadcast, commercial, industrial, educational video, print projects, multimedia,
10 | games, and the internet, so long as substantial value is added to the content. (For example,
11 | incorporating an audio clip into a commercial qualifies, while reposting our audio clip on
12 | YouTube with no modification or no visual component does not.) Once you download a file it is
13 | yours to keep and use forever, royalty- free, even if you change your subscription or cancel
14 | your account.
15 |
16 | List of applicable sound files
17 |
18 | chimeconnect.wav
19 | chimedisconnect.wav
20 | errormessage.wav
21 | warningmessage.wav
--------------------------------------------------------------------------------
/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt:
--------------------------------------------------------------------------------
1 | EXHIBIT A - LEGO® Open Source License Agreement
2 |
3 | The contents of the file 'nxtstartupsound.wav' contained in this SDK are subject to the
4 | LEGO® Open Source License Agreement Version 1.0 (the "License"); you may not use this
5 | file except in compliance with the License. You may obtain a copy of the License
6 | at "LEGO Open Source License.pdf" contained in the same directory as this exhibit.
7 |
8 | Software distributed under the License is distributed on an "AS IS" basis, WITHOUT
9 | WARRANTY OF ANY KIND, either express or implied. See the License for the specific
10 | language governing rights and limitations under the License.
11 |
12 | The Original Code is \AT91SAM7S256\Resource\SOUNDS\!Startup.rso.
13 | LEGO is the owner of the Original Code. Portions created by Robert Atkinson are
14 | Copyright (C) 2015. All Rights Reserved.
15 | Contributor(s): Robert Atkinson.
--------------------------------------------------------------------------------
/doc/legal/LEGO Open Source License.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/legal/LEGO Open Source License.pdf
--------------------------------------------------------------------------------
/doc/media/PullRequest.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/PullRequest.PNG
--------------------------------------------------------------------------------
/doc/media/chips.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/chips.jpg
--------------------------------------------------------------------------------
/doc/media/readme.md:
--------------------------------------------------------------------------------
1 | ## Vuforia Localizer Sample Image Targets
2 |
3 | The files in this folder are the printable images that correspond to the sample
4 | trackable data sets that have been included with integration of Vuforia to the
5 | FTC SDK. See the ConceptVuforiaNavigation.java OpMode for details.
6 |
--------------------------------------------------------------------------------
/doc/media/stones.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/stones.jpg
--------------------------------------------------------------------------------
/doc/media/target_chips_A4.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/target_chips_A4.pdf
--------------------------------------------------------------------------------
/doc/media/target_chips_USLetter.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/target_chips_USLetter.pdf
--------------------------------------------------------------------------------
/doc/media/target_stones_A4.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/target_stones_A4.pdf
--------------------------------------------------------------------------------
/doc/media/target_stones_USLetter.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/doc/media/target_stones_USLetter.pdf
--------------------------------------------------------------------------------
/doc/tutorial/README.txt:
--------------------------------------------------------------------------------
1 | What happened to the FTC Software Development Tutorials???
2 |
3 | In an effort to save space and reduce download time, the older (and outdated) tutorial documents (FTCTraining_Manual.pdf, FTC_FieldCoordinateSystemDefinition.pdf, FTC_NextGenGuide.pdf, and FTC_ZTE_ChannelChange.pdf) have been removed.
4 |
5 | You can find the releveant PDF files attached to the "Releases" page of the GitHub repository:
6 |
7 | https://github.com/ftctechnh/ftc_app/releases
8 |
--------------------------------------------------------------------------------
/gradle.properties:
--------------------------------------------------------------------------------
1 | # AndroidX package structure to make it clearer which packages are bundled with the
2 | # Android operating system, and which are packaged with your app's APK
3 | # https://developer.android.com/topic/libraries/support-library/androidx-rn
4 | android.useAndroidX=true
5 |
6 | # Automatically convert third-party libraries to use AndroidX
7 | android.enableJetifier=true
8 |
--------------------------------------------------------------------------------
/gradle/wrapper/gradle-wrapper.jar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/gradle/wrapper/gradle-wrapper.jar
--------------------------------------------------------------------------------
/gradle/wrapper/gradle-wrapper.properties:
--------------------------------------------------------------------------------
1 | #Fri Jul 24 14:30:03 PDT 2020
2 | distributionBase=GRADLE_USER_HOME
3 | distributionPath=wrapper/dists
4 | zipStoreBase=GRADLE_USER_HOME
5 | zipStorePath=wrapper/dists
6 | distributionUrl=https\://services.gradle.org/distributions/gradle-6.1.1-all.zip
7 |
--------------------------------------------------------------------------------
/gradlew:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env bash
2 |
3 | ##############################################################################
4 | ##
5 | ## Gradle start up script for UN*X
6 | ##
7 | ##############################################################################
8 |
9 | # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
10 | DEFAULT_JVM_OPTS=""
11 |
12 | APP_NAME="Gradle"
13 | APP_BASE_NAME=`basename "$0"`
14 |
15 | # Use the maximum available, or set MAX_FD != -1 to use that value.
16 | MAX_FD="maximum"
17 |
18 | warn ( ) {
19 | echo "$*"
20 | }
21 |
22 | die ( ) {
23 | echo
24 | echo "$*"
25 | echo
26 | exit 1
27 | }
28 |
29 | # OS specific support (must be 'true' or 'false').
30 | cygwin=false
31 | msys=false
32 | darwin=false
33 | case "`uname`" in
34 | CYGWIN* )
35 | cygwin=true
36 | ;;
37 | Darwin* )
38 | darwin=true
39 | ;;
40 | MINGW* )
41 | msys=true
42 | ;;
43 | esac
44 |
45 | # For Cygwin, ensure paths are in UNIX format before anything is touched.
46 | if $cygwin ; then
47 | [ -n "$JAVA_HOME" ] && JAVA_HOME=`cygpath --unix "$JAVA_HOME"`
48 | fi
49 |
50 | # Attempt to set APP_HOME
51 | # Resolve links: $0 may be a link
52 | PRG="$0"
53 | # Need this for relative symlinks.
54 | while [ -h "$PRG" ] ; do
55 | ls=`ls -ld "$PRG"`
56 | link=`expr "$ls" : '.*-> \(.*\)$'`
57 | if expr "$link" : '/.*' > /dev/null; then
58 | PRG="$link"
59 | else
60 | PRG=`dirname "$PRG"`"/$link"
61 | fi
62 | done
63 | SAVED="`pwd`"
64 | cd "`dirname \"$PRG\"`/" >&-
65 | APP_HOME="`pwd -P`"
66 | cd "$SAVED" >&-
67 |
68 | CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
69 |
70 | # Determine the Java command to use to start the JVM.
71 | if [ -n "$JAVA_HOME" ] ; then
72 | if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
73 | # IBM's JDK on AIX uses strange locations for the executables
74 | JAVACMD="$JAVA_HOME/jre/sh/java"
75 | else
76 | JAVACMD="$JAVA_HOME/bin/java"
77 | fi
78 | if [ ! -x "$JAVACMD" ] ; then
79 | die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
80 |
81 | Please set the JAVA_HOME variable in your environment to match the
82 | location of your Java installation."
83 | fi
84 | else
85 | JAVACMD="java"
86 | which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
87 |
88 | Please set the JAVA_HOME variable in your environment to match the
89 | location of your Java installation."
90 | fi
91 |
92 | # Increase the maximum file descriptors if we can.
93 | if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then
94 | MAX_FD_LIMIT=`ulimit -H -n`
95 | if [ $? -eq 0 ] ; then
96 | if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
97 | MAX_FD="$MAX_FD_LIMIT"
98 | fi
99 | ulimit -n $MAX_FD
100 | if [ $? -ne 0 ] ; then
101 | warn "Could not set maximum file descriptor limit: $MAX_FD"
102 | fi
103 | else
104 | warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
105 | fi
106 | fi
107 |
108 | # For Darwin, add options to specify how the application appears in the dock
109 | if $darwin; then
110 | GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
111 | fi
112 |
113 | # For Cygwin, switch paths to Windows format before running java
114 | if $cygwin ; then
115 | APP_HOME=`cygpath --path --mixed "$APP_HOME"`
116 | CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
117 |
118 | # We build the pattern for arguments to be converted via cygpath
119 | ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
120 | SEP=""
121 | for dir in $ROOTDIRSRAW ; do
122 | ROOTDIRS="$ROOTDIRS$SEP$dir"
123 | SEP="|"
124 | done
125 | OURCYGPATTERN="(^($ROOTDIRS))"
126 | # Add a user-defined pattern to the cygpath arguments
127 | if [ "$GRADLE_CYGPATTERN" != "" ] ; then
128 | OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
129 | fi
130 | # Now convert the arguments - kludge to limit ourselves to /bin/sh
131 | i=0
132 | for arg in "$@" ; do
133 | CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
134 | CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option
135 |
136 | if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition
137 | eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
138 | else
139 | eval `echo args$i`="\"$arg\""
140 | fi
141 | i=$((i+1))
142 | done
143 | case $i in
144 | (0) set -- ;;
145 | (1) set -- "$args0" ;;
146 | (2) set -- "$args0" "$args1" ;;
147 | (3) set -- "$args0" "$args1" "$args2" ;;
148 | (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
149 | (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
150 | (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
151 | (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
152 | (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
153 | (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
154 | esac
155 | fi
156 |
157 | # Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules
158 | function splitJvmOpts() {
159 | JVM_OPTS=("$@")
160 | }
161 | eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS
162 | JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME"
163 |
164 | exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@"
165 |
--------------------------------------------------------------------------------
/gradlew.bat:
--------------------------------------------------------------------------------
1 | @if "%DEBUG%" == "" @echo off
2 | @rem ##########################################################################
3 | @rem
4 | @rem Gradle startup script for Windows
5 | @rem
6 | @rem ##########################################################################
7 |
8 | @rem Set local scope for the variables with windows NT shell
9 | if "%OS%"=="Windows_NT" setlocal
10 |
11 | @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
12 | set DEFAULT_JVM_OPTS=
13 |
14 | set DIRNAME=%~dp0
15 | if "%DIRNAME%" == "" set DIRNAME=.
16 | set APP_BASE_NAME=%~n0
17 | set APP_HOME=%DIRNAME%
18 |
19 | @rem Find java.exe
20 | if defined JAVA_HOME goto findJavaFromJavaHome
21 |
22 | set JAVA_EXE=java.exe
23 | %JAVA_EXE% -version >NUL 2>&1
24 | if "%ERRORLEVEL%" == "0" goto init
25 |
26 | echo.
27 | echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
28 | echo.
29 | echo Please set the JAVA_HOME variable in your environment to match the
30 | echo location of your Java installation.
31 |
32 | goto fail
33 |
34 | :findJavaFromJavaHome
35 | set JAVA_HOME=%JAVA_HOME:"=%
36 | set JAVA_EXE=%JAVA_HOME%/bin/java.exe
37 |
38 | if exist "%JAVA_EXE%" goto init
39 |
40 | echo.
41 | echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
42 | echo.
43 | echo Please set the JAVA_HOME variable in your environment to match the
44 | echo location of your Java installation.
45 |
46 | goto fail
47 |
48 | :init
49 | @rem Get command-line arguments, handling Windowz variants
50 |
51 | if not "%OS%" == "Windows_NT" goto win9xME_args
52 | if "%@eval[2+2]" == "4" goto 4NT_args
53 |
54 | :win9xME_args
55 | @rem Slurp the command line arguments.
56 | set CMD_LINE_ARGS=
57 | set _SKIP=2
58 |
59 | :win9xME_args_slurp
60 | if "x%~1" == "x" goto execute
61 |
62 | set CMD_LINE_ARGS=%*
63 | goto execute
64 |
65 | :4NT_args
66 | @rem Get arguments from the 4NT Shell from JP Software
67 | set CMD_LINE_ARGS=%$
68 |
69 | :execute
70 | @rem Setup the command line
71 |
72 | set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
73 |
74 | @rem Execute Gradle
75 | "%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
76 |
77 | :end
78 | @rem End local scope for the variables with windows NT shell
79 | if "%ERRORLEVEL%"=="0" goto mainEnd
80 |
81 | :fail
82 | rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
83 | rem the _cmd.exe /c_ return code!
84 | if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
85 | exit /b 1
86 |
87 | :mainEnd
88 | if "%OS%"=="Windows_NT" endlocal
89 |
90 | :omega
91 |
--------------------------------------------------------------------------------
/libs/Analytics-release.aar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/Analytics-release.aar
--------------------------------------------------------------------------------
/libs/README.txt:
--------------------------------------------------------------------------------
1 | Location of external libs
2 |
--------------------------------------------------------------------------------
/libs/Vuforia.jar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/Vuforia.jar
--------------------------------------------------------------------------------
/libs/WirelessP2p-release.aar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/WirelessP2p-release.aar
--------------------------------------------------------------------------------
/libs/arm64-v8a/libVuforia.so:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/arm64-v8a/libVuforia.so
--------------------------------------------------------------------------------
/libs/armeabi-v7a/libVuforia.so:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/armeabi-v7a/libVuforia.so
--------------------------------------------------------------------------------
/libs/ftc.debug.keystore:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/ftc.debug.keystore
--------------------------------------------------------------------------------
/libs/tensorflow-lite-0.0.0-nightly.aar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/tensorflow-lite-0.0.0-nightly.aar
--------------------------------------------------------------------------------
/libs/tfod-release.aar:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/FTCLib/RoadRunner-FTCLib-Quickstart/7490585daf79ef9da1858472619ff7185ad60c7b/libs/tfod-release.aar
--------------------------------------------------------------------------------
/settings.gradle:
--------------------------------------------------------------------------------
1 | include ':FtcRobotController'
2 | include ':TeamCode'
3 |
--------------------------------------------------------------------------------