waypoints = new LinkedList<>();
11 | private int targetIdx = 1;
12 | private boolean finished;
13 |
14 | public PurePursuitPath(Waypoint... ws) {
15 | if (ws.length < 2) throw new IllegalArgumentException();
16 | Collections.addAll(waypoints, ws);
17 | if(waypoints.getLast().getType() != Waypoint.Type.POSE) throw new IllegalArgumentException();
18 | }
19 |
20 | public double[] xPoints() {
21 | double[] res = new double[waypoints.size()];
22 | int i = 0;
23 | for (Waypoint pt : waypoints) {
24 | res[i] = pt.getPoint().x;
25 | i++;
26 | }
27 | return res;
28 | }
29 | public double[] yPoints() {
30 | double[] res = new double[waypoints.size()];
31 | int i = 0;
32 | for (Waypoint pt : waypoints) {
33 | res[i] = pt.getPoint().y;
34 | i++;
35 | }
36 | return res;
37 | }
38 |
39 | public Pose update(Pose robot) {
40 | Waypoint prev = waypoints.get(targetIdx - 1);
41 | Waypoint target = waypoints.get(targetIdx);
42 |
43 | double distance = robot.distanceTo(target.getPoint());
44 |
45 | if(distance > target.getRadius()){
46 | Point intersection = PurePursuitUtil.lineCircleIntersection(
47 | prev.getPoint(), target.getPoint(), robot, target.getRadius());
48 | Pose targetPose;
49 |
50 | if(target.getType() == Waypoint.Type.POSE){
51 | targetPose = new Pose(intersection, ((Pose)target.getPoint()).heading);
52 | }else{
53 | double robotAngle = AngleUnit.normalizeRadians(robot.heading);
54 | double forwardAngle = -intersection.subtract(robot).atan();
55 | double backwardsAngle = AngleUnit.normalizeRadians(forwardAngle + Math.PI);
56 |
57 | double autoAngle =
58 | Math.abs(AngleUnit.normalizeRadians(robotAngle - forwardAngle)) <
59 | Math.abs(AngleUnit.normalizeRadians(robotAngle - backwardsAngle)) ?
60 | forwardAngle : backwardsAngle;
61 |
62 | targetPose = new Pose(intersection, autoAngle);
63 | }
64 |
65 | return targetPose;
66 | }else{
67 | if(targetIdx == waypoints.size() - 1){
68 | finished = true;
69 | return getEndPose();
70 | }else{
71 | targetIdx++;
72 | return update(robot);
73 | }
74 | }
75 | }
76 |
77 | public boolean isFinished(){
78 | return finished;
79 | }
80 |
81 | public Pose getEndPose(){
82 | return (Pose) waypoints.getLast().getPoint();
83 | }
84 |
85 | public double getRadius(){
86 | return waypoints.get(targetIdx).getRadius();
87 | }
88 | }
--------------------------------------------------------------------------------
/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml:
--------------------------------------------------------------------------------
1 |
33 |
34 |
79 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.robotcontroller.external.samples;
2 |
3 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
4 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
5 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
6 | import org.firstinspires.ftc.robotcore.external.Telemetry;
7 |
8 | /*
9 | * This OpMode illustrates using the touchpad feature found on some gamepads.
10 | *
11 | * The Sony PS4 gamepad can detect two distinct touches on the central touchpad.
12 | * Other gamepads with different touchpads may provide mixed results.
13 | *
14 | * The touchpads are accessed through the standard gamepad1 and gamepad2 objects.
15 | * Several new members were added to the Gamepad class in FTC SDK Rev 7
16 | *
17 | * .touchpad_finger_1 returns true if at least one finger is detected.
18 | * .touchpad_finger_1_x finger 1 X coordinate. Valid if touchpad_finger_1 is true
19 | * .touchpad_finger_1_y finger 1 Y coordinate. Valid if touchpad_finger_1 is true
20 | *
21 | * .touchpad_finger_2 returns true if a second finger is detected
22 | * .touchpad_finger_2_x finger 2 X coordinate. Valid if touchpad_finger_2 is true
23 | * .touchpad_finger_2_y finger 2 Y coordinate. Valid if touchpad_finger_2 is true
24 | *
25 | * Finger touches are reported with an X and Y coordinate in following coordinate system.
26 | *
27 | * 1) X is the Horizontal axis, and Y is the vertical axis
28 | * 2) The 0,0 origin is at the center of the touchpad.
29 | * 3) 1.0, 1.0 is at the top right corner of the touchpad.
30 | * 4) -1.0,-1.0 is at the bottom left corner of the touchpad.
31 | *
32 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
33 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
34 | */
35 |
36 | @Disabled
37 | @TeleOp(name="Concept: Gamepad Touchpad", group ="Concept")
38 | public class ConceptGamepadTouchpad extends LinearOpMode
39 | {
40 | @Override
41 | public void runOpMode()
42 | {
43 | telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
44 |
45 | telemetry.addData(">", "Press Start");
46 | telemetry.update();
47 |
48 | waitForStart();
49 |
50 | while (opModeIsActive())
51 | {
52 | boolean finger = false;
53 |
54 | // Display finger 1 x & y position if finger detected
55 | if(gamepad1.touchpad_finger_1)
56 | {
57 | finger = true;
58 | telemetry.addLine(String.format("Finger 1: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_1_x, gamepad1.touchpad_finger_1_y));
59 | }
60 |
61 | // Display finger 2 x & y position if finger detected
62 | if(gamepad1.touchpad_finger_2)
63 | {
64 | finger = true;
65 | telemetry.addLine(String.format("Finger 2: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_2_x, gamepad1.touchpad_finger_2_y));
66 | }
67 |
68 | if(!finger)
69 | {
70 | telemetry.addLine("No fingers");
71 | }
72 |
73 | telemetry.update();
74 | sleep(10);
75 | }
76 | }
77 | }
78 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/RoadrunnerTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.tuning;
2 |
3 | import com.acmerobotics.roadrunner.Pose2d;
4 | import com.acmerobotics.roadrunner.SequentialAction;
5 | import com.acmerobotics.roadrunner.Vector2d;
6 | import com.acmerobotics.roadrunner.ftc.Actions;
7 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
8 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
9 |
10 | import org.firstinspires.ftc.teamcode.MecanumDrive;
11 | import org.firstinspires.ftc.teamcode.pathing.Pose;
12 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitCommand;
13 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitPath;
14 | import org.firstinspires.ftc.teamcode.pathing.Waypoint;
15 | import org.firstinspires.ftc.teamcode.subsystems.Intake;
16 |
17 | @TeleOp(group = "Test")
18 | public final class RoadrunnerTest extends LinearOpMode {
19 | @Override
20 | public void runOpMode() throws InterruptedException {
21 | MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(12, 63, Math.toRadians(-90)));
22 | Intake intake = new Intake(hardwareMap);
23 | intake.initialize();
24 |
25 | waitForStart();
26 |
27 | Actions.runBlocking(
28 | new SequentialAction(
29 | drive.actionBuilder(new Pose2d(12, 63, Math.toRadians(-90)))
30 | .strafeToLinearHeading(new Vector2d(24, 10), Math.toRadians(180))
31 | .setTangent(Math.toRadians(180))
32 | .splineToConstantHeading(new Vector2d(-36, 10), Math.toRadians(180))
33 | .splineToConstantHeading(new Vector2d(-55, 8), Math.toRadians(180))
34 | .build(),
35 | drive.actionBuilder(new Pose2d(-55, 8, Math.toRadians(180)))
36 | .setReversed(true)
37 | .splineToConstantHeading(new Vector2d(-36, 10), Math.toRadians(180) - Math.PI)
38 | .splineToConstantHeading(new Vector2d(24, 10), Math.toRadians(180) - Math.PI)
39 | .splineToSplineHeading(new Pose2d(51, 21.5, Math.toRadians(200)), Math.toRadians(200) - Math.PI)
40 | .build(),
41 | drive.actionBuilder(new Pose2d(51, 21.5, Math.toRadians(200)))
42 | .splineToSplineHeading(new Pose2d(24, 10, Math.toRadians(180)), Math.toRadians(180))
43 | .splineToConstantHeading(new Vector2d(-36, 10), Math.toRadians(180))
44 | .splineToConstantHeading(new Vector2d(-55, 8), Math.toRadians(180))
45 | .build(),
46 | drive.actionBuilder(new Pose2d(-55, 8, Math.toRadians(180)))
47 | .setReversed(true)
48 | .splineToConstantHeading(new Vector2d(-36, 10), Math.toRadians(180) - Math.PI)
49 | .splineToConstantHeading(new Vector2d(24, 10), Math.toRadians(180) - Math.PI)
50 | .splineToSplineHeading(new Pose2d(53, 21.5, Math.toRadians(200)), Math.toRadians(200) - Math.PI)
51 | .build()
52 | ));
53 | }
54 | }
55 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/SmartGameTimer.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.qualcomm.robotcore.util.ElapsedTime;
4 |
5 | import org.firstinspires.ftc.teamcode.subsystems.Memory;
6 |
7 | import java.util.Locale;
8 |
9 | public class SmartGameTimer {
10 | private double offset;
11 | private TIMER_START_MODE mode;
12 | private ElapsedTime elapsedTime;
13 |
14 | public enum TIMER_START_MODE {
15 | STANDARD, // standard game timer 2:00
16 | FILE_RECOVERED, // based off of auto start time
17 | STANDARD_FALLBACK // fallback to standard when file time is too early or nonexistant
18 | }
19 |
20 | public SmartGameTimer(boolean standardOperation) {
21 | if (standardOperation) {
22 | this.offset = 0;
23 | this.elapsedTime = new ElapsedTime();
24 | mode = TIMER_START_MODE.STANDARD;
25 | } else {
26 | try {
27 | // get the value from memory
28 | String autoStartTimeStr = Memory.getStringFromFile(Memory.SAVED_TIME_FILE_NAME);
29 | long autoStartTime = Long.parseLong(autoStartTimeStr);
30 | long calculatedTeleopStartTime = autoStartTime + 38 * 1000;
31 |
32 | long currentTime = System.currentTimeMillis();
33 | offset = (currentTime - calculatedTeleopStartTime) * 0.001;
34 |
35 | if (offset > 120) { // if game already ended, fallback
36 | this.mode = TIMER_START_MODE.STANDARD_FALLBACK;
37 | this.offset = 0;
38 | this.elapsedTime = new ElapsedTime();
39 | } else {
40 | this.mode = TIMER_START_MODE.FILE_RECOVERED;
41 | this.elapsedTime = new ElapsedTime();
42 | }
43 | } catch (Exception e) {
44 | this.mode = TIMER_START_MODE.STANDARD_FALLBACK;
45 | this.offset = 0;
46 | this.elapsedTime = new ElapsedTime();
47 | }
48 | }
49 | }
50 |
51 | public double seconds() {
52 | return elapsedTime.seconds() + offset;
53 | }
54 |
55 | /**
56 | * Formats time as -mm:ss. e.g. " 01:30" or "-00:27")
57 | */
58 | public String formattedString() {
59 | int secLeft = (int) (120 - seconds());
60 | return String.format(Locale.ENGLISH, "%s%02d:%02d", secLeft<0?"-":" ", Math.abs(secLeft / 60), Math.abs(secLeft % 60));
61 | }
62 |
63 | public void reset() {
64 | offset = 0;
65 | elapsedTime.reset();
66 | }
67 |
68 | public void resetIfStandard() {
69 | if (mode == TIMER_START_MODE.STANDARD || mode == TIMER_START_MODE.STANDARD_FALLBACK) {
70 | reset();
71 | }
72 | }
73 |
74 | public boolean isNominal() {
75 | return mode == TIMER_START_MODE.STANDARD;
76 | }
77 |
78 | public boolean isRecovered() {
79 | return mode == TIMER_START_MODE.FILE_RECOVERED;
80 | }
81 |
82 | public boolean isLikelyWrong() {
83 | return mode == TIMER_START_MODE.STANDARD_FALLBACK;
84 | }
85 |
86 | public String status() {
87 | if (isRecovered()) return "recovered";
88 | if (isNominal()) return "normal";
89 | if (isLikelyWrong()) return "no reference";
90 | return "unknown state";
91 | }
92 | }
93 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/res/values/strings.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
38 |
39 |
40 |
41 | FTC Robot Controller
42 |
43 |
44 | Self Inspect
45 | Program & Manage
46 | Blocks
47 | Settings
48 | Restart Robot
49 | Configure Robot
50 | About
51 | Exit
52 |
53 |
54 | Configuration Complete
55 | Restarting Robot
56 | The Robot Controller must be fully up and running before entering Program and Manage Mode.
57 |
58 |
59 |
60 | - @style/AppThemeRedRC
61 | - @style/AppThemeGreenRC
62 | - @style/AppThemeBlueRC
63 | - @style/AppThemePurpleRC
64 | - @style/AppThemeOrangeRC
65 | - @style/AppThemeTealRC
66 |
67 |
68 | pref_ftc_season_year_of_current_rc_new
69 |
70 | @string/packageNameRobotController
71 |
72 |
73 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PurePursuitTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.tuning;
2 |
3 | import com.acmerobotics.roadrunner.Pose2d;
4 | import com.acmerobotics.roadrunner.SequentialAction;
5 | import com.acmerobotics.roadrunner.Vector2d;
6 | import com.acmerobotics.roadrunner.ftc.Actions;
7 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
8 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
9 |
10 | import org.firstinspires.ftc.teamcode.MecanumDrive;
11 | import org.firstinspires.ftc.teamcode.pathing.Point;
12 | import org.firstinspires.ftc.teamcode.pathing.Pose;
13 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitCommand;
14 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitPath;
15 | import org.firstinspires.ftc.teamcode.pathing.Waypoint;
16 | import org.firstinspires.ftc.teamcode.subsystems.Intake;
17 |
18 | @TeleOp(group = "Test")
19 | public final class PurePursuitTest extends LinearOpMode {
20 | @Override
21 | public void runOpMode() throws InterruptedException {
22 | MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(12, 63, Math.toRadians(-90)));
23 | Intake intake = new Intake(hardwareMap);
24 | intake.initialize();
25 |
26 | waitForStart();
27 |
28 | Actions.runBlocking(
29 | new SequentialAction(
30 | new PurePursuitCommand(drive, new PurePursuitPath(
31 | new Waypoint(new Pose(12, 63, Math.toRadians(-90)), 15),
32 |
33 | // Drive to stack
34 | new Waypoint(new Pose(24, 10, Math.toRadians(180)), 15),
35 | new Waypoint(new Pose(-36, 10, Math.toRadians(180)), 15),
36 | new Waypoint(new Pose(-55, 8, Math.toRadians(180)), 15)
37 | )),
38 | new PurePursuitCommand(drive, new PurePursuitPath(
39 | // Score
40 | new Waypoint(new Pose(-55, 8, Math.toRadians(180)), 15),
41 | new Waypoint(new Pose(-36, 10, Math.toRadians(180)), 15),
42 | new Waypoint(new Pose(24, 10, Math.toRadians(180)), 15),
43 | new Waypoint(new Pose(51, 21.5, Math.toRadians(200)), 15)
44 | )),
45 | new PurePursuitCommand(drive, new PurePursuitPath(
46 | // Drive to stack
47 | new Waypoint(new Pose(51, 21.5, Math.toRadians(200)), 15),
48 | new Waypoint(new Pose(24, 10, Math.toRadians(180)), 15),
49 | new Waypoint(new Pose(-36, 10, Math.toRadians(180)), 15),
50 | new Waypoint(new Pose(-55, 8, Math.toRadians(180)), 15)
51 | )),
52 | new PurePursuitCommand(drive, new PurePursuitPath(
53 | // Score
54 | new Waypoint(new Pose(-55, 8, Math.toRadians(180)), 15),
55 | new Waypoint(new Pose(-36, 10, Math.toRadians(180)), 15),
56 | new Waypoint(new Pose(24, 10, Math.toRadians(180)), 15),
57 | new Waypoint(new Pose(51, 21.5, Math.toRadians(200)), 15)
58 | ))
59 | ));
60 | }
61 | }
62 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2014, 2015 Qualcomm Technologies Inc
2 |
3 | All rights reserved.
4 |
5 | Redistribution and use in source and binary forms, with or without modification,
6 | are permitted (subject to the limitations in the disclaimer below) provided that
7 | the following conditions are met:
8 |
9 | Redistributions of source code must retain the above copyright notice, this list
10 | of conditions and the following disclaimer.
11 |
12 | Redistributions in binary form must reproduce the above copyright notice, this
13 | list of conditions and the following disclaimer in the documentation and/or
14 | other materials provided with the distribution.
15 |
16 | Neither the name of Qualcomm Technologies Inc nor the names of its contributors
17 | may be used to endorse or promote products derived from this software without
18 | specific prior written permission.
19 |
20 | NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
21 | LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
23 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
31 |
32 | package org.firstinspires.ftc.robotcontroller.internal;
33 |
34 | import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
35 | import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
36 |
37 | import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp;
38 |
39 | /**
40 | * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game.
41 | * @see #register(OpModeManager)
42 | */
43 | public class FtcOpModeRegister implements OpModeRegister {
44 |
45 | /**
46 | * {@link #register(OpModeManager)} is called by the SDK game in order to register
47 | * OpMode classes or instances that will participate in an FTC game.
48 | *
49 | * There are two mechanisms by which an OpMode may be registered.
50 | *
51 | * 1) The preferred method is by means of class annotations in the OpMode itself.
52 | * See, for example the class annotations in {@link ConceptNullOp}.
53 | *
54 | * 2) The other, retired, method is to modify this {@link #register(OpModeManager)}
55 | * method to include explicit calls to OpModeManager.register().
56 | * This method of modifying this file directly is discouraged, as it
57 | * makes updates to the SDK harder to integrate into your code.
58 | *
59 | * @param manager the object which contains methods for carrying out OpMode registrations
60 | *
61 | * @see com.qualcomm.robotcore.eventloop.opmode.TeleOp
62 | * @see com.qualcomm.robotcore.eventloop.opmode.Autonomous
63 | */
64 | public void register(OpModeManager manager) {
65 |
66 | /**
67 | * Any manual OpMode class registrations should go here.
68 | */
69 | }
70 | }
71 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
36 |
37 | /*
38 | * This OpMode shows how to use a Modern Robotics Optical Distance Sensor
39 | * It assumes that the ODS sensor is configured with a name of "sensor_ods".
40 | *
41 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
42 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
43 | */
44 | @TeleOp(name = "Sensor: MR ODS", group = "Sensor")
45 | @Disabled
46 | public class SensorMROpticalDistance extends LinearOpMode {
47 |
48 | OpticalDistanceSensor odsSensor; // Hardware Device Object
49 |
50 | @Override
51 | public void runOpMode() {
52 |
53 | // get a reference to our Light Sensor object.
54 | odsSensor = hardwareMap.get(OpticalDistanceSensor.class, "sensor_ods");
55 |
56 | // wait for the start button to be pressed.
57 | waitForStart();
58 |
59 | // while the OpMode is active, loop and read the light levels.
60 | // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
61 | while (opModeIsActive()) {
62 |
63 | // send the info back to driver station using telemetry function.
64 | telemetry.addData("Raw", odsSensor.getRawLightDetected());
65 | telemetry.addData("Normal", odsSensor.getLightDetected());
66 |
67 | telemetry.update();
68 | }
69 | }
70 | }
71 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.OpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.util.ElapsedTime;
36 |
37 | /*
38 | * Demonstrates an empty iterative OpMode
39 | */
40 | @TeleOp(name = "Concept: NullOp", group = "Concept")
41 | @Disabled
42 | public class ConceptNullOp extends OpMode {
43 |
44 | private ElapsedTime runtime = new ElapsedTime();
45 |
46 | /**
47 | * This method will be called once, when the INIT button is pressed.
48 | */
49 | @Override
50 | public void init() {
51 | telemetry.addData("Status", "Initialized");
52 | }
53 |
54 | /**
55 | * This method will be called repeatedly during the period between when
56 | * the init button is pressed and when the play button is pressed (or the
57 | * OpMode is stopped).
58 | */
59 | @Override
60 | public void init_loop() {
61 | }
62 |
63 | /**
64 | * This method will be called once, when the play button is pressed.
65 | */
66 | @Override
67 | public void start() {
68 | runtime.reset();
69 | }
70 |
71 | /**
72 | * This method will be called repeatedly during the period between when
73 | * the play button is pressed and when the OpMode is stopped.
74 | */
75 | @Override
76 | public void loop() {
77 | telemetry.addData("Status", "Run Time: " + runtime.toString());
78 | }
79 |
80 | /**
81 | * This method will be called once, when this OpMode is stopped.
82 | *
83 | * Your ability to control hardware from this method will be limited.
84 | */
85 | @Override
86 | public void stop() {
87 |
88 | }
89 | }
90 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/tests/DriveTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.opmode.tests;
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.Pose2d;
7 | import com.acmerobotics.roadrunner.PoseVelocity2d;
8 | import com.acmerobotics.roadrunner.Vector2d;
9 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
10 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
11 | import com.qualcomm.robotcore.hardware.DcMotor;
12 | import com.qualcomm.robotcore.hardware.DcMotorEx;
13 | import com.qualcomm.robotcore.hardware.DcMotorSimple;
14 | import com.qualcomm.robotcore.util.Range;
15 |
16 | import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
17 | import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
18 | import org.firstinspires.ftc.teamcode.MecanumDrive;
19 | import org.firstinspires.ftc.teamcode.util.GamePadController;
20 | import org.firstinspires.ftc.teamcode.util.HardwareCreator;
21 |
22 | import java.util.ArrayList;
23 | import java.util.Collections;
24 | import java.util.List;
25 |
26 | /**
27 | * This is a test program for testing any motor under any run mode
28 | */
29 | @Config
30 | //@Disabled
31 | @TeleOp(group = "Test")
32 | public class DriveTest extends LinearOpMode {
33 | public static double DRIVE_X = 0;
34 | public static double DRIVE_Y = 0;
35 | public static double DRIVE_TURN = 0;
36 |
37 | @Override
38 | public void runOpMode() throws InterruptedException {
39 |
40 | // Communicate to dashboard
41 | telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
42 |
43 | // Pull a list of all the motors from the configuration
44 | MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
45 |
46 | // Create our controller interface
47 | GamePadController g1 = new GamePadController(gamepad1);
48 |
49 | telemetry.addData("leftFrontCurrent", drive.leftFront.getCurrent(CurrentUnit.AMPS));
50 | telemetry.addData("rightFrontCurrent", drive.rightFront.getCurrent(CurrentUnit.AMPS));
51 | telemetry.addData("rightBackCurrent", drive.rightBack.getCurrent(CurrentUnit.AMPS));
52 | telemetry.addData("leftBackCurrent", drive.leftBack.getCurrent(CurrentUnit.AMPS));
53 |
54 | // Get input
55 | waitForStart();
56 |
57 | // exit immediately if stopped
58 | if (isStopRequested()) return;
59 |
60 | // Game loop
61 | while (!isStopRequested()) {
62 | // Update controller
63 | g1.update();
64 |
65 | // Control mode
66 | if (g1.left_stick_y != 0) {
67 | DRIVE_X = -g1.left_stick_y;
68 | }
69 | if (g1.left_stick_x != 0) {
70 | DRIVE_Y = -g1.left_stick_x;
71 | }
72 | if (g1.left_trigger != 0 || g1.right_trigger != 0) {
73 | DRIVE_TURN = g1.left_trigger - g1.right_trigger;
74 | }
75 | drive.setDrivePowers(new PoseVelocity2d(new Vector2d(DRIVE_X, DRIVE_Y), DRIVE_TURN));
76 |
77 | // Telemetry
78 | telemetry.addData("leftFrontCurrent", drive.leftFront.getCurrent(CurrentUnit.AMPS));
79 | telemetry.addData("rightFrontCurrent", drive.rightFront.getCurrent(CurrentUnit.AMPS));
80 | telemetry.addData("rightBackCurrent", drive.rightBack.getCurrent(CurrentUnit.AMPS));
81 | telemetry.addData("leftBackCurrent", drive.leftBack.getCurrent(CurrentUnit.AMPS));
82 | telemetry.update();
83 | }
84 | }
85 | }
86 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
33 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
34 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
35 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
36 | import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
37 |
38 | /*
39 | * This OpMode illustrates how to use the Modern Robotics Range Sensor.
40 | *
41 | * The OpMode assumes that the range sensor is configured with a name of "sensor_range".
42 | *
43 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
44 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
45 | *
46 | * @see MR Range Sensor
47 | */
48 | @TeleOp(name = "Sensor: MR range sensor", group = "Sensor")
49 | @Disabled // comment out or remove this line to enable this OpMode
50 | public class SensorMRRangeSensor extends LinearOpMode {
51 |
52 | ModernRoboticsI2cRangeSensor rangeSensor;
53 |
54 | @Override public void runOpMode() {
55 |
56 | // get a reference to our compass
57 | rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "sensor_range");
58 |
59 | // wait for the start button to be pressed
60 | waitForStart();
61 |
62 | while (opModeIsActive()) {
63 | telemetry.addData("raw ultrasonic", rangeSensor.rawUltrasonic());
64 | telemetry.addData("raw optical", rangeSensor.rawOptical());
65 | telemetry.addData("cm optical", "%.2f cm", rangeSensor.cmOptical());
66 | telemetry.addData("cm", "%.2f cm", rangeSensor.getDistance(DistanceUnit.CM));
67 | telemetry.update();
68 | }
69 | }
70 | }
71 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PurePursuitTestUnderTruss.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.tuning;
2 |
3 | import com.acmerobotics.roadrunner.Pose2d;
4 | import com.acmerobotics.roadrunner.SequentialAction;
5 | import com.acmerobotics.roadrunner.ftc.Actions;
6 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
7 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
8 |
9 | import org.firstinspires.ftc.teamcode.MecanumDrive;
10 | import org.firstinspires.ftc.teamcode.pathing.Pose;
11 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitCommand;
12 | import org.firstinspires.ftc.teamcode.pathing.PurePursuitPath;
13 | import org.firstinspires.ftc.teamcode.pathing.Waypoint;
14 | import org.firstinspires.ftc.teamcode.subsystems.Intake;
15 |
16 | @TeleOp(group = "Test")
17 | public final class PurePursuitTestUnderTruss extends LinearOpMode {
18 | @Override
19 | public void runOpMode() throws InterruptedException {
20 | MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(12, 63, Math.toRadians(-90)));
21 | Intake intake = new Intake(hardwareMap);
22 | intake.initialize();
23 |
24 | waitForStart();
25 |
26 | Actions.runBlocking(
27 | new SequentialAction(
28 | new PurePursuitCommand(drive, new PurePursuitPath(
29 | new Waypoint(new Pose(12, 63, Math.toRadians(-90)), 15),
30 |
31 | // Drive to score
32 | new Waypoint(new Pose(52, 44, Math.toRadians(160)), 15)
33 | )),
34 | new PurePursuitCommand(drive, new PurePursuitPath(
35 | // Drive to stack
36 | new Waypoint(new Pose(55, 44, Math.toRadians(160)), 20),
37 | new Waypoint(new Pose(26, 60, Math.toRadians(180)), 20),
38 | new Waypoint(new Pose(-34, 60, Math.toRadians(180)), 20),
39 | new Waypoint(new Pose(-60, 37.5, Math.toRadians(210)), 15)
40 | )),
41 | new PurePursuitCommand(drive, new PurePursuitPath(
42 | // Score
43 | new Waypoint(new Pose(-60, 37.5, Math.toRadians(210)), 20),
44 | new Waypoint(new Pose(-34, 61, Math.toRadians(180)), 20),
45 | new Waypoint(new Pose(26, 61, Math.toRadians(180)), 20),
46 | new Waypoint(new Pose(52, 44, Math.toRadians(160)), 15)
47 | )),
48 | new PurePursuitCommand(drive, new PurePursuitPath(
49 | // Drive to stack
50 | new Waypoint(new Pose(55, 44, Math.toRadians(160)), 20),
51 | new Waypoint(new Pose(26, 60, Math.toRadians(180)), 20),
52 | new Waypoint(new Pose(-34, 60, Math.toRadians(180)), 20),
53 | new Waypoint(new Pose(-60, 37.5, Math.toRadians(210)), 15)
54 | )),
55 | new PurePursuitCommand(drive, new PurePursuitPath(
56 | // Score
57 | new Waypoint(new Pose(-61, 37.5, Math.toRadians(210)), 20),
58 | new Waypoint(new Pose(-34, 61, Math.toRadians(180)), 20),
59 | new Waypoint(new Pose(26, 61, Math.toRadians(180)), 20),
60 | new Waypoint(new Pose(52, 44, Math.toRadians(160)), 15)
61 | ))
62 | ));
63 | }
64 | }
65 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/tests/PartnerPreloadTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.opmode.tests;
2 |
3 | import android.util.Size;
4 |
5 | import com.acmerobotics.dashboard.FtcDashboard;
6 | import com.acmerobotics.dashboard.config.Config;
7 | import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
8 | import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
9 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
10 |
11 | import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
12 | import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
13 | import org.firstinspires.ftc.teamcode.subsystems.vision.PartnerPreloadProcessor;
14 | import org.firstinspires.ftc.teamcode.util.CameraUtil;
15 | import org.firstinspires.ftc.vision.VisionPortal;
16 | import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
17 |
18 | @Config
19 | @Autonomous(name = "PartnerPreloadTest", group = "Test")
20 | public class PartnerPreloadTest extends LinearOpMode {
21 | public static boolean RED = false;
22 | public static int SPIKE = 0; // 0 = right, 1 = middle, 2 = left
23 | public static boolean FALLBACK = false;
24 | public CameraUtil.DebugMode debugMode = CameraUtil.DebugMode.Dashboard;
25 | public void runOpMode() throws InterruptedException {
26 | telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
27 |
28 | AprilTagProcessor aprilTag = PartnerPreloadProcessor.newAprilTagProcessor();
29 | PartnerPreloadProcessor processor = new PartnerPreloadProcessor(aprilTag);
30 | processor.setDebugMode(debugMode);
31 |
32 | VisionPortal portal = new VisionPortal.Builder()
33 | // Get the actual camera on the robot, add the processor, state the orientation of the camera.
34 | .setCamera(hardwareMap.get(WebcamName.class, "WebcamOuttake"))
35 | .setCameraResolution(new Size(1280, 720)) // THIS CRASHES AT 1920x1080, GL ERROR: Out of memory!
36 | .addProcessor(aprilTag)
37 | .addProcessor(processor)
38 | .enableLiveView(true)
39 | .setAutoStopLiveView(true)
40 | .build();
41 |
42 | if (debugMode == CameraUtil.DebugMode.Dashboard) {
43 | FtcDashboard.getInstance().startCameraStream(processor, 0);
44 | }
45 |
46 | while (opModeInInit()) {
47 | processor.updateTarget(SPIKE, RED);
48 | processor.fallback = FALLBACK;
49 |
50 | telemetry.addData("Red Side", RED);
51 | telemetry.addData("Spike Position", SPIKE);
52 | telemetry.addData("Target April Tag", processor.targetAprilTag);
53 | telemetry.addLine();
54 |
55 | telemetry.addData("April Tag Detected", processor.detecting);
56 | telemetry.addData("Left Average", processor.leftZoneAverage);
57 | telemetry.addData("Right Average", processor.rightZoneAverage);
58 | telemetry.addLine();
59 |
60 | telemetry.addData("Preload Left", processor.preloadLeft);
61 |
62 | if (processor.detecting) {
63 | telemetry.addData("x", processor.detectedPose.x);
64 | telemetry.addData("y", processor.detectedPose.y);
65 | telemetry.addData("z", processor.detectedPose.z);
66 | telemetry.addLine();
67 | }
68 |
69 | telemetry.update();
70 | }
71 |
72 | waitForStart();
73 |
74 | portal.stopStreaming();
75 |
76 | while (opModeIsActive()) {
77 | // the camera processor got result only when it is in active
78 | telemetry.addData("loop:", "loop");
79 | telemetry.update();
80 | }
81 | }
82 | }
83 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.hardware.TouchSensor;
36 |
37 | /*
38 | * This OpMode demonstrates how to use a REV Robotics Touch Sensor, REV Robotics Magnetic Limit Switch, or other device
39 | * that implements the TouchSensor interface. Any touch sensor that connects its output to ground when pressed
40 | * (known as "active low") can be configured as a "REV Touch Sensor". This includes REV's Magnetic Limit Switch.
41 | *
42 | * The OpMode assumes that the touch sensor is configured with a name of "sensor_touch".
43 | *
44 | * A REV Robotics Touch Sensor must be configured on digital port number 1, 3, 5, or 7.
45 | * A Magnetic Limit Switch can be configured on any digital port.
46 | *
47 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
48 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
49 | */
50 | @TeleOp(name = "Sensor: REV touch sensor", group = "Sensor")
51 | @Disabled
52 | public class SensorTouch extends LinearOpMode {
53 | TouchSensor touchSensor; // Touch sensor Object
54 |
55 | @Override
56 | public void runOpMode() {
57 |
58 | // get a reference to our touchSensor object.
59 | touchSensor = hardwareMap.get(TouchSensor.class, "sensor_touch");
60 |
61 | // wait for the start button to be pressed.
62 | waitForStart();
63 |
64 | // while the OpMode is active, loop and read whether the sensor is being pressed.
65 | // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
66 | while (opModeIsActive()) {
67 |
68 | // send the info back to driver station using telemetry function.
69 | if (touchSensor.isPressed()) {
70 | telemetry.addData("Touch Sensor", "Is Pressed");
71 | } else {
72 | telemetry.addData("Touch Sensor", "Is Not Pressed");
73 | }
74 |
75 | telemetry.update();
76 | }
77 | }
78 | }
79 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/res/xml/app_settings.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
40 |
41 |
45 |
46 |
48 |
49 |
55 |
56 |
64 |
65 |
71 |
72 |
76 |
80 |
81 |
82 |
86 |
89 |
90 |
91 |
92 |
93 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java:
--------------------------------------------------------------------------------
1 | /*
2 | Copyright (c) 2018 FIRST
3 |
4 | All rights reserved.
5 |
6 | Redistribution and use in source and binary forms, with or without modification,
7 | are permitted (subject to the limitations in the disclaimer below) provided that
8 | the following conditions are met:
9 |
10 | Redistributions of source code must retain the above copyright notice, this list
11 | of conditions and the following disclaimer.
12 |
13 | Redistributions in binary form must reproduce the above copyright notice, this
14 | list of conditions and the following disclaimer in the documentation and/or
15 | other materials provided with the distribution.
16 |
17 | Neither the name of FIRST nor the names of its contributors may be used to
18 | endorse or promote products derived from this software without specific prior
19 | written permission.
20 |
21 | NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
22 | LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
24 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
26 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
30 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
31 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 | */
33 | package org.firstinspires.ftc.robotcontroller.external.samples;
34 |
35 | import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
36 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
37 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
38 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
39 | import com.qualcomm.robotcore.hardware.DistanceSensor;
40 | import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
41 |
42 | /*
43 | * This OpMode illustrates how to use the REV Robotics 2M Distance Sensor.
44 | *
45 | * The OpMode assumes that the sensor is configured with a name of "sensor_distance".
46 | *
47 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
48 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
49 | *
50 | * See the sensor's product page: https://www.revrobotics.com/rev-31-1505/
51 | */
52 | @TeleOp(name = "Sensor: REV2mDistance", group = "Sensor")
53 | @Disabled
54 | public class SensorREV2mDistance extends LinearOpMode {
55 |
56 | private DistanceSensor sensorDistance;
57 |
58 | @Override
59 | public void runOpMode() {
60 | // you can use this as a regular DistanceSensor.
61 | sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance");
62 |
63 | // you can also cast this to a Rev2mDistanceSensor if you want to use added
64 | // methods associated with the Rev2mDistanceSensor class.
65 | Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor) sensorDistance;
66 |
67 | telemetry.addData(">>", "Press start to continue");
68 | telemetry.update();
69 |
70 | waitForStart();
71 | while(opModeIsActive()) {
72 | // generic DistanceSensor methods.
73 | telemetry.addData("deviceName", sensorDistance.getDeviceName() );
74 | telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM)));
75 | telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM)));
76 | telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER)));
77 | telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH)));
78 |
79 | // Rev2mDistanceSensor specific methods.
80 | telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID()));
81 | telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur()));
82 |
83 | telemetry.update();
84 | }
85 | }
86 |
87 | }
88 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java:
--------------------------------------------------------------------------------
1 | /*
2 | * Copyright (c) 2018 Craig MacFarlane
3 | *
4 | * All rights reserved.
5 | *
6 | * Redistribution and use in source and binary forms, with or without modification, are permitted
7 | * (subject to the limitations in the disclaimer below) provided that the following conditions are
8 | * met:
9 | *
10 | * Redistributions of source code must retain the above copyright notice, this list of conditions
11 | * and the following disclaimer.
12 | *
13 | * Redistributions in binary form must reproduce the above copyright notice, this list of conditions
14 | * and the following disclaimer in the documentation and/or other materials provided with the
15 | * distribution.
16 | *
17 | * Neither the name of Craig MacFarlane nor the names of its contributors may be used to
18 | * endorse or promote products derived from this software without specific prior written permission.
19 | *
20 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS
21 | * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
22 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
26 | * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
28 | * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 | */
30 | package org.firstinspires.ftc.robotcontroller.internal;
31 |
32 | import android.Manifest;
33 | import android.os.Bundle;
34 |
35 | import com.qualcomm.ftcrobotcontroller.R;
36 |
37 | import org.firstinspires.ftc.robotcore.internal.system.Misc;
38 | import org.firstinspires.ftc.robotcore.internal.system.PermissionValidatorActivity;
39 |
40 | import java.util.ArrayList;
41 | import java.util.List;
42 |
43 | public class PermissionValidatorWrapper extends PermissionValidatorActivity {
44 |
45 | private final String TAG = "PermissionValidatorWrapper";
46 |
47 | /*
48 | * The list of dangerous permissions the robot controller needs.
49 | */
50 | protected List robotControllerPermissions = new ArrayList() {{
51 | add(Manifest.permission.WRITE_EXTERNAL_STORAGE);
52 | add(Manifest.permission.READ_EXTERNAL_STORAGE);
53 | add(Manifest.permission.CAMERA);
54 | add(Manifest.permission.ACCESS_COARSE_LOCATION);
55 | add(Manifest.permission.ACCESS_FINE_LOCATION);
56 | add(Manifest.permission.READ_PHONE_STATE);
57 | }};
58 |
59 | private final static Class startApplication = FtcRobotControllerActivity.class;
60 |
61 | public String mapPermissionToExplanation(final String permission) {
62 | if (permission.equals(Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
63 | return Misc.formatForUser(R.string.permRcWriteExternalStorageExplain);
64 | } else if (permission.equals(Manifest.permission.READ_EXTERNAL_STORAGE)) {
65 | return Misc.formatForUser(R.string.permRcReadExternalStorageExplain);
66 | } else if (permission.equals(Manifest.permission.CAMERA)) {
67 | return Misc.formatForUser(R.string.permRcCameraExplain);
68 | } else if (permission.equals(Manifest.permission.ACCESS_COARSE_LOCATION)) {
69 | return Misc.formatForUser(R.string.permAccessLocationExplain);
70 | } else if (permission.equals(Manifest.permission.ACCESS_FINE_LOCATION)) {
71 | return Misc.formatForUser(R.string.permAccessLocationExplain);
72 | } else if (permission.equals(Manifest.permission.READ_PHONE_STATE)) {
73 | return Misc.formatForUser(R.string.permReadPhoneState);
74 | }
75 | return Misc.formatForUser(R.string.permGenericExplain);
76 | }
77 |
78 | @Override
79 | protected void onCreate(Bundle savedInstanceState)
80 | {
81 | super.onCreate(savedInstanceState);
82 |
83 | permissions = robotControllerPermissions;
84 | }
85 |
86 | protected Class onStartApplication()
87 | {
88 | FtcRobotControllerActivity.setPermissionsValidated();
89 | return startApplication;
90 | }
91 | }
92 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/HardwareCreator.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 | import com.acmerobotics.dashboard.config.Config;
4 | import com.qualcomm.robotcore.hardware.CRServo;
5 | import com.qualcomm.robotcore.hardware.DcMotorEx;
6 | import com.qualcomm.robotcore.hardware.DigitalChannel;
7 | import com.qualcomm.robotcore.hardware.HardwareMap;
8 | import com.qualcomm.robotcore.hardware.PwmControl;
9 | import com.qualcomm.robotcore.hardware.Servo;
10 | import com.qualcomm.robotcore.hardware.ServoImplEx;
11 | import com.qualcomm.robotcore.util.RobotLog;
12 |
13 | import org.firstinspires.ftc.teamcode.util.fakes.CRServoFake;
14 | import org.firstinspires.ftc.teamcode.util.fakes.DcMotorFake;
15 | import org.firstinspires.ftc.teamcode.util.fakes.DigitalChannelFake;
16 | import org.firstinspires.ftc.teamcode.util.fakes.ServoFake;
17 |
18 |
19 | /**
20 | * Class that creates most of our hardware objects
21 | * Intercepts simulation and substitutes in a fake hardware device
22 | * Intercepts deviceNotFound and gives warning and a fake device
23 | */
24 | @Config
25 | public class HardwareCreator {
26 | public static boolean SIMULATE_HARDWARE = false;
27 | public static boolean SIMULATE_DRIVETRAIN = false;
28 |
29 | public enum ServoType {
30 | DEFAULT(600, 2400),
31 | GOBILDA(500, 2500),
32 | AXON(600, 2400); // technically can go 500-2500, but can end up with a continuous servo
33 |
34 | public final PwmControl.PwmRange pwmRange;
35 |
36 | ServoType(double usPulseLower, double usPulseUpper) {
37 | this.pwmRange = new PwmControl.PwmRange(usPulseLower, usPulseUpper);
38 | }
39 | }
40 |
41 | public static DcMotorEx createMotor(HardwareMap hardwareMap, String deviceName) {
42 | if (SIMULATE_HARDWARE) return new DcMotorFake();
43 | try {
44 | return hardwareMap.get(DcMotorEx.class, deviceName);
45 | } catch (IllegalArgumentException e) { // Could not find device
46 | RobotLog.addGlobalWarningMessage("Failed to find DcMotorEx '%s'", deviceName);
47 | return new DcMotorFake();
48 | }
49 | }
50 |
51 | public static DigitalChannel createDigitalChannel(HardwareMap hardwareMap, String deviceName) {
52 | if (SIMULATE_HARDWARE) return new DigitalChannelFake();
53 | try {
54 | return hardwareMap.digitalChannel.get(deviceName);
55 | } catch (IllegalArgumentException e) { // Could not find device
56 | RobotLog.addGlobalWarningMessage("Failed to find DigitalChannel '%s'", deviceName);
57 | return new DigitalChannelFake();
58 | }
59 | }
60 |
61 | public static Servo createServo(HardwareMap hardwareMap, String deviceName) {
62 | return createServo(hardwareMap, deviceName, ServoType.DEFAULT);
63 | }
64 |
65 | public static Servo createServo(HardwareMap hardwareMap, String deviceName, ServoType type) {
66 | if (SIMULATE_HARDWARE) return setServoRange(new ServoFake(), type);
67 | try {
68 | return setServoRange(hardwareMap.get(ServoImplEx.class, deviceName), type);
69 | } catch (IllegalArgumentException e) {
70 | RobotLog.addGlobalWarningMessage("Failed to find Servo '%s'", deviceName);
71 | return setServoRange(new ServoFake(), type);
72 | }
73 | }
74 |
75 | public static CRServo createCRServo(HardwareMap hardwareMap, String deviceName) {
76 | return createCRServo(hardwareMap, deviceName, ServoType.DEFAULT);
77 | }
78 |
79 | public static CRServo createCRServo(HardwareMap hardwareMap, String deviceName, ServoType type) {
80 | if (SIMULATE_HARDWARE) return setServoRange(new CRServoFake(), type);
81 | try {
82 | return setServoRange(hardwareMap.get(CRServo.class, deviceName), type);
83 | } catch (IllegalArgumentException e) {
84 | RobotLog.addGlobalWarningMessage("Failed to find CRServo '%s'", deviceName);
85 | return setServoRange(new CRServoFake(), type);
86 | }
87 | }
88 |
89 | public static Servo setServoRange(Servo servo, ServoType servoType) {
90 | if (servo instanceof PwmControl) {
91 | ((PwmControl) servo).setPwmRange(servoType.pwmRange);
92 | }
93 | return servo;
94 | }
95 |
96 | public static CRServo setServoRange(CRServo crServo, ServoType servoType) {
97 | if (crServo instanceof PwmControl) {
98 | ((PwmControl) crServo).setPwmRange(servoType.pwmRange);
99 | }
100 | return crServo;
101 | }
102 | }
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md:
--------------------------------------------------------------------------------
1 | ## Sample Class/Opmode conventions
2 | #### V 1.1.0 8/9/2017
3 |
4 | This document defines the FTC Sample OpMode and Class conventions.
5 |
6 | ### OpMode Name
7 |
8 | To gain a better understanding of how the samples are organized, and how to interpret the
9 | naming system, it will help to understand the conventions that were used during their creation.
10 |
11 | To summarize: A range of different samples classes will reside in the java/external/samples.
12 | The class names will follow a naming convention which indicates the purpose of each class.
13 | The prefix of the name will be one of the following:
14 |
15 | Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
16 | of a particular style of OpMode. These are bare bones examples.
17 |
18 | Sensor: This is a Sample OpMode that shows how to use a specific sensor.
19 | It is not intended to drive a functioning robot, it is simply showing the minimal code
20 | required to read and display the sensor values.
21 |
22 | Robot: This is a Sample OpMode that assumes a simple two-motorWithEncoder (differential) drive base.
23 | It may be used to provide a common baseline driving OpMode, or
24 | to demonstrate how a particular sensor or concept can be used to navigate.
25 |
26 | Concept: This is a sample OpMode that illustrates performing a specific function or concept.
27 | These may be complex, but their operation should be explained clearly in the comments,
28 | or the comments should reference an external doc, guide or tutorial.
29 | Each OpMode should try to only demonstrate a single concept so they are easy to
30 | locate based on their name. These OpModes may not produce a drivable robot.
31 |
32 | After the prefix, other conventions will apply:
33 |
34 | * Sensor class names should constructed as: Sensor - Company - Type
35 | * Robot class names should be constructed as: Robot - Mode - Action - OpModetype
36 | * Concept class names should be constructed as: Concept - Topic - OpModetype
37 |
38 | ### Sample OpMode Content/Style
39 |
40 | Code is formatted as per the Google Style Guide:
41 |
42 | https://google.github.io/styleguide/javaguide.html
43 |
44 | With “Sensor” and “Hardware” samples, the code should demonstrate the essential function,
45 | and not be embellished with too much additional “clever” code. If a sensor has special
46 | addressing needs, or has a variety of modes or outputs, these should be demonstrated as
47 | simply as possible.
48 |
49 | Special programming methods, or robot control techniques should be reserved for “Concept” Samples,
50 | and where possible, Samples should strive to only demonstrate a single concept,
51 | eg: State machine coding, or a User Menu system, and not combine them into a single “all inclusive”
52 | sample. This will prevent an “all inclusive” Sample being deleted just because one part of it
53 | becomes obsolete.
54 |
55 | ### Device Configuration Names
56 |
57 | The following device names are used in the external samples
58 |
59 | ** Motors:
60 | left_drive
61 | right_drive
62 | left_arm
63 |
64 | ** Servos:
65 | left_hand
66 | right_hand
67 | arm
68 | claw
69 |
70 | ** Sensors:
71 | sensor_color
72 | sensor_ir
73 | sensor_light
74 | sensor_ods
75 | sensor_range
76 | sensor_touch
77 | sensor_color_distance
78 | sensor_digital
79 | digin
80 | digout
81 |
82 | ** Localization:
83 | compass
84 | gyro
85 | imu
86 | navx
87 |
88 | ### Device Object Names
89 |
90 | Device Object names should use the same words as the device’s configuration name, but they
91 | should be re-structured to be a suitable Java variable name. This should keep the same word order,
92 | but adopt the style of beginning with a lower case letter, and then each subsequent word
93 | starting with an upper case letter.
94 |
95 | Eg: from the examples above: tool, leftMotor, rightClawServo, rearLightSensor.
96 |
97 | Note: Sometimes it’s helpful to put the device type first, followed by the variant.
98 | eg: motorLeft and motorRight, but this should only be done if the same word order
99 | is used on the device configuration name.
100 |
101 | ### OpMode code Comments
102 |
103 | Sample comments should read like normal code comments, that is, as an explanation of what the
104 | sample code is doing. They should NOT be directives to the user,
105 | like: “insert your joystick code here” as these comments typically aren’t
106 | detailed enough to be useful. They also often get left in the code and become garbage.
107 |
108 | Instead, an example of the joystick code should be shown with a comment describing what it is doing.
109 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/tests/ServoTest.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.opmode.tests;
2 |
3 | import com.acmerobotics.dashboard.FtcDashboard;
4 | import com.acmerobotics.dashboard.config.Config;
5 | import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
6 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
7 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
8 | import com.qualcomm.robotcore.hardware.Servo;
9 | import com.qualcomm.robotcore.util.Range;
10 |
11 | import org.firstinspires.ftc.teamcode.util.GamePadController;
12 | import org.firstinspires.ftc.teamcode.util.HardwareCreator;
13 |
14 | import java.util.ArrayList;
15 | import java.util.Collections;
16 | import java.util.List;
17 |
18 | /**
19 | * This is a test program for tuning any servo's positions.
20 | */
21 | @Config
22 | //@Disabled
23 | @TeleOp(group = "Test")
24 | public class ServoTest extends LinearOpMode {
25 | public static String NAME = "servo";
26 | public static boolean REVERSED = false;
27 | public static HardwareCreator.ServoType SERVO_TYPE = HardwareCreator.ServoType.DEFAULT;
28 | public static double POSITION = 0.5;
29 |
30 | private List servoNames;
31 | private static int servoIndex = -1;
32 |
33 | @Override
34 | public void runOpMode() throws InterruptedException {
35 |
36 | // Communicate to dashboard
37 | telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
38 |
39 | // Pull a list of all the servos from the configuration
40 | servoNames = new ArrayList<>(hardwareMap.servo.size());
41 | hardwareMap.servo.entrySet().forEach(entry -> servoNames.add(entry.getKey()));
42 | Collections.sort(servoNames);
43 | if (!servoNames.contains(NAME)) servoIndex = -1; // reset index if dashboard changed it
44 |
45 | // Create our controller interface
46 | GamePadController g1 = new GamePadController(gamepad1);
47 |
48 | // Get input
49 | while (opModeInInit()) {
50 | // Control servo selection
51 | g1.update();
52 | if (g1.leftBumperOnce()) {
53 | servoIndex = Range.clip(servoIndex - 1, 0, servoNames.size() - 1);
54 | NAME = servoNames.get(servoIndex);
55 | }
56 | if (g1.rightBumperOnce()) {
57 | servoIndex = Range.clip(servoIndex + 1, 0, servoNames.size() - 1);
58 | NAME = servoNames.get(servoIndex);
59 | }
60 | // Control reverse
61 | if (g1.aOnce()) REVERSED = !REVERSED;
62 | // Control init position
63 | double joystick = -g1.right_stick_y;
64 | if (Math.abs(joystick) > 0.01) POSITION = Range.clip(POSITION + 0.0001*Math.pow(joystick, 3), 0, 1);
65 | if (g1.rightStickButtonOnce()) POSITION = 0.5;
66 |
67 | // Telemetry state and instructions
68 | telemetry.addLine("~ Use bumpers to scroll through servo list");
69 | telemetry.addLine("~ Use A to reverse the servo");
70 | telemetry.addLine("~ Use right joystick to adjust the init position");
71 | telemetry.addLine();
72 | if (!servoNames.contains(NAME)) telemetry.addLine("*** THIS SERVO DOES NOT EXIST ***");
73 | telemetry.addData("Servo name", NAME);
74 | telemetry.addData("Servo direction", (REVERSED ? "REVERSE" : "FORWARD"));
75 | telemetry.addData("Servo type", SERVO_TYPE);
76 | telemetry.addData("Init Position", POSITION);
77 | telemetry.update();
78 | }
79 |
80 | // Exit immediately if stopped
81 | if (isStopRequested()) return;
82 |
83 | // Create the servo
84 | Servo servo = HardwareCreator.createServo(hardwareMap, NAME, SERVO_TYPE);
85 | servo.setDirection(REVERSED ? Servo.Direction.REVERSE : Servo.Direction.FORWARD);
86 |
87 | // Game loop
88 | while (!isStopRequested()) {
89 | // Update controller
90 | g1.update();
91 |
92 | // Control position
93 | double joystick = -g1.right_stick_y;
94 | if (Math.abs(joystick) > 0.01) POSITION = Range.clip(POSITION + 0.001*Math.pow(joystick, 3), 0, 1);
95 | if (g1.dpadUpOnce()) POSITION = Range.clip(POSITION + 0.01, 0, 1);
96 | if (g1.dpadDownOnce()) POSITION = Range.clip(POSITION - 0.01, 0, 1);
97 |
98 | // Set position
99 | servo.setPosition(POSITION);
100 |
101 | // Telemetry state and instructions
102 | telemetry.addLine("~ Adjust servo position with the dpad or right joystick");
103 | telemetry.addLine();
104 | telemetry.addData("Position", servo.getPosition());
105 | telemetry.addData("Servo name", NAME);
106 | telemetry.addData("Servo direction", (REVERSED ? "REVERSE" : "FORWARD"));
107 | telemetry.addData("Servo type", SERVO_TYPE);
108 | telemetry.update();
109 | }
110 | }
111 | }
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.hardware.DcMotor;
36 |
37 | /*
38 | * This OpMode ramps a single motor speed up and down repeatedly until Stop is pressed.
39 | * The code is structured as a LinearOpMode
40 | *
41 | * This code assumes a DC motor configured with the name "left_drive" as is found on a Robot.
42 | *
43 | * INCREMENT sets how much to increase/decrease the power each cycle
44 | * CYCLE_MS sets the update period.
45 | *
46 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
47 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
48 | */
49 | @TeleOp(name = "Concept: Ramp Motor Speed", group = "Concept")
50 | @Disabled
51 | public class ConceptRampMotorSpeed extends LinearOpMode {
52 |
53 | static final double INCREMENT = 0.01; // amount to ramp motor each CYCLE_MS cycle
54 | static final int CYCLE_MS = 50; // period of each cycle
55 | static final double MAX_FWD = 1.0; // Maximum FWD power applied to motor
56 | static final double MAX_REV = -1.0; // Maximum REV power applied to motor
57 |
58 | // Define class members
59 | DcMotor motor;
60 | double power = 0;
61 | boolean rampUp = true;
62 |
63 |
64 | @Override
65 | public void runOpMode() {
66 |
67 | // Connect to motor (Assume standard left wheel)
68 | // Change the text in quotes to match any motor name on your robot.
69 | motor = hardwareMap.get(DcMotor.class, "left_drive");
70 |
71 | // Wait for the start button
72 | telemetry.addData(">", "Press Start to run Motors." );
73 | telemetry.update();
74 | waitForStart();
75 |
76 | // Ramp motor speeds till stop pressed.
77 | while(opModeIsActive()) {
78 |
79 | // Ramp the motors, according to the rampUp variable.
80 | if (rampUp) {
81 | // Keep stepping up until we hit the max value.
82 | power += INCREMENT ;
83 | if (power >= MAX_FWD ) {
84 | power = MAX_FWD;
85 | rampUp = !rampUp; // Switch ramp direction
86 | }
87 | }
88 | else {
89 | // Keep stepping down until we hit the min value.
90 | power -= INCREMENT ;
91 | if (power <= MAX_REV ) {
92 | power = MAX_REV;
93 | rampUp = !rampUp; // Switch ramp direction
94 | }
95 | }
96 |
97 | // Display the current value
98 | telemetry.addData("Motor Power", "%5.2f", power);
99 | telemetry.addData(">", "Press Stop to end test." );
100 | telemetry.update();
101 |
102 | // Set the motor to the new power and pause;
103 | motor.setPower(power);
104 | sleep(CYCLE_MS);
105 | idle();
106 | }
107 |
108 | // Turn off motor and signal done;
109 | motor.setPower(0);
110 | telemetry.addData(">", "Done");
111 | telemetry.update();
112 |
113 | }
114 | }
115 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/control/PIDFController.kt:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util.control
2 |
3 | import kotlin.math.abs
4 | import kotlin.math.max
5 | import kotlin.math.min
6 | import kotlin.math.sign
7 |
8 | const val EPSILON = 1e-6
9 |
10 | infix fun Double.epsilonEquals(other: Double) = abs(this - other) < EPSILON
11 |
12 | /**
13 | * PID controller with various feedforward components.
14 | */
15 | class PIDFController
16 | /**
17 | * Constructor for [PIDFController]. [kV], [kA], and [kStatic] are designed for DC motor feedforward
18 | * control (the most common kind of feedforward in FTC). [kF] provides a custom feedforward term for other plants.
19 | *
20 | * @param pid traditional PID coefficients
21 | * @param kV feedforward velocity gain
22 | * @param kA feedforward acceleration gain
23 | * @param kStatic additive feedforward constant
24 | * @param kF custom feedforward that depends on position and/or velocity (e.g., a gravity term for arms)
25 | * @param clock clock
26 | */
27 | @JvmOverloads constructor(
28 | private val pid: PIDCoefficients,
29 | private val kV: Double = 0.0,
30 | private val kA: Double = 0.0,
31 | private val kStatic: Double = 0.0,
32 | private val kF: (Double, Double, Double?) -> Double = { _, _, _ -> 0.0 },
33 | ) {
34 | private var errorSum: Double = 0.0
35 | private var lastUpdateTimestamp: Double = Double.NaN
36 |
37 | private var inputBounded: Boolean = false
38 | private var minInput: Double = 0.0
39 | private var maxInput: Double = 0.0
40 |
41 | private var outputBounded: Boolean = false
42 | private var minOutput: Double = 0.0
43 | private var maxOutput: Double = 0.0
44 |
45 | /**
46 | * Target position (that is, the controller setpoint).
47 | */
48 | var targetPosition: Double = 0.0
49 |
50 | /**
51 | * Target velocity.
52 | */
53 | var targetVelocity: Double = 0.0
54 |
55 | /**
56 | * Target acceleration.
57 | */
58 | var targetAcceleration: Double = 0.0
59 |
60 | /**
61 | * Error computed in the last call to [update].
62 | */
63 | var lastError: Double = 0.0
64 | private set
65 |
66 | /**
67 | * Sets bound on the input of the controller. The min and max values are considered modularly-equivalent (that is,
68 | * the input wraps around).
69 | *
70 | * @param min minimum input
71 | * @param max maximum input
72 | */
73 | fun setInputBounds(min: Double, max: Double) {
74 | if (min < max) {
75 | inputBounded = true
76 | minInput = min
77 | maxInput = max
78 | }
79 | }
80 |
81 | /**
82 | * Sets bounds on the output of the controller.
83 | *
84 | * @param min minimum output
85 | * @param max maximum output
86 | */
87 | fun setOutputBounds(min: Double, max: Double) {
88 | if (min < max) {
89 | outputBounded = true
90 | minOutput = min
91 | maxOutput = max
92 | }
93 | }
94 |
95 | private fun getPositionError(measuredPosition: Double): Double {
96 | var error = targetPosition - measuredPosition
97 | if (inputBounded) {
98 | val inputRange = maxInput - minInput
99 | while (abs(error) > inputRange / 2.0) {
100 | error -= sign(error) * inputRange
101 | }
102 | }
103 | return error
104 | }
105 |
106 | /**
107 | * Run a single iteration of the controller.
108 | *
109 | * @param measuredPosition measured position (feedback)
110 | * @param measuredVelocity measured velocity
111 | */
112 | @JvmOverloads
113 | fun update(
114 | measuredPosition: Double,
115 | measuredVelocity: Double? = null
116 | ): Double {
117 | val currentTimestamp = System.nanoTime() / 1e9
118 | val error = getPositionError(measuredPosition)
119 | return if (lastUpdateTimestamp.isNaN()) {
120 | lastError = error
121 | lastUpdateTimestamp = currentTimestamp
122 | 0.0
123 | } else {
124 | val dt = currentTimestamp - lastUpdateTimestamp
125 | errorSum += 0.5 * (error + lastError) * dt
126 | val errorDeriv = (error - lastError) / dt
127 |
128 | lastError = error
129 | lastUpdateTimestamp = currentTimestamp
130 |
131 | // note: we'd like to refactor this with Kinematics.calculateMotorFeedforward() but kF complicates the
132 | // determination of the sign of kStatic
133 | val baseOutput = pid.kP * error + pid.kI * errorSum +
134 | pid.kD * (measuredVelocity?.let { targetVelocity - it } ?: errorDeriv) +
135 | kV * targetVelocity + kA * targetAcceleration + kF(targetPosition, measuredPosition, measuredVelocity)
136 | val output = if (baseOutput epsilonEquals 0.0) 0.0 else baseOutput + sign(baseOutput) * kStatic
137 |
138 | if (outputBounded) {
139 | max(minOutput, min(output, maxOutput))
140 | } else {
141 | output
142 | }
143 | }
144 | }
145 |
146 | /**
147 | * Reset the controller's integral sum.
148 | */
149 | fun reset() {
150 | errorSum = 0.0
151 | lastError = 0.0
152 | lastUpdateTimestamp = Double.NaN
153 | }
154 | }
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2017 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.hardware.Servo;
36 |
37 | /*
38 | * This OpMode scans a single servo back and forward until Stop is pressed.
39 | * The code is structured as a LinearOpMode
40 | * INCREMENT sets how much to increase/decrease the servo position each cycle
41 | * CYCLE_MS sets the update period.
42 | *
43 | * This code assumes a Servo configured with the name "left_hand" as is found on a Robot.
44 | *
45 | * NOTE: When any servo position is set, ALL attached servos are activated, so ensure that any other
46 | * connected servos are able to move freely before running this test.
47 | *
48 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
49 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
50 | */
51 | @TeleOp(name = "Concept: Scan Servo", group = "Concept")
52 | @Disabled
53 | public class ConceptScanServo extends LinearOpMode {
54 |
55 | static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle
56 | static final int CYCLE_MS = 50; // period of each cycle
57 | static final double MAX_POS = 1.0; // Maximum rotational position
58 | static final double MIN_POS = 0.0; // Minimum rotational position
59 |
60 | // Define class members
61 | Servo servo;
62 | double position = (MAX_POS - MIN_POS) / 2; // Start at halfway position
63 | boolean rampUp = true;
64 |
65 |
66 | @Override
67 | public void runOpMode() {
68 |
69 | // Connect to servo (Assume Robot Left Hand)
70 | // Change the text in quotes to match any servo name on your robot.
71 | servo = hardwareMap.get(Servo.class, "left_hand");
72 |
73 | // Wait for the start button
74 | telemetry.addData(">", "Press Start to scan Servo." );
75 | telemetry.update();
76 | waitForStart();
77 |
78 |
79 | // Scan servo till stop pressed.
80 | while(opModeIsActive()){
81 |
82 | // slew the servo, according to the rampUp (direction) variable.
83 | if (rampUp) {
84 | // Keep stepping up until we hit the max value.
85 | position += INCREMENT ;
86 | if (position >= MAX_POS ) {
87 | position = MAX_POS;
88 | rampUp = !rampUp; // Switch ramp direction
89 | }
90 | }
91 | else {
92 | // Keep stepping down until we hit the min value.
93 | position -= INCREMENT ;
94 | if (position <= MIN_POS ) {
95 | position = MIN_POS;
96 | rampUp = !rampUp; // Switch ramp direction
97 | }
98 | }
99 |
100 | // Display the current value
101 | telemetry.addData("Servo Position", "%5.2f", position);
102 | telemetry.addData(">", "Press Stop to end test." );
103 | telemetry.update();
104 |
105 | // Set the servo to the new position and pause;
106 | servo.setPosition(position);
107 | sleep(CYCLE_MS);
108 | idle();
109 | }
110 |
111 | // Signal done;
112 | telemetry.addData(">", "Done");
113 | telemetry.update();
114 | }
115 | }
116 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/MotorWithVelocityPID.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.util;
2 |
3 |
4 | import android.util.Log;
5 |
6 | import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
7 | import com.acmerobotics.roadrunner.Action;
8 | import com.qualcomm.robotcore.hardware.DcMotor;
9 | import com.qualcomm.robotcore.hardware.DcMotorEx;
10 | import com.qualcomm.robotcore.hardware.DcMotorSimple;
11 | import com.qualcomm.robotcore.util.Range;
12 |
13 | import org.firstinspires.ftc.teamcode.util.control.PIDCoefficients;
14 | import org.firstinspires.ftc.teamcode.util.control.PIDFController;
15 |
16 | import kotlin.jvm.functions.Function3;
17 |
18 | public class MotorWithVelocityPID {
19 | private DcMotorEx motor;
20 | private PIDFController pidfController;
21 | private PIDCoefficients pid;
22 | private int targetVelocity = 0;
23 | private int internalOffset = 0;
24 | private double maxPower = 0;
25 | private double power = 0;
26 |
27 | public MotorWithVelocityPID(DcMotorEx motor, PIDCoefficients pid) {
28 | this(motor, pid, (t, x, v) -> 0.0);
29 | }
30 |
31 | public MotorWithVelocityPID(DcMotorEx motor, PIDCoefficients pid, Function3 f) {
32 | this.motor = motor;
33 | this.pid = pid;
34 | this.pidfController = new PIDFController(pid, 0, 0, 0, f);
35 |
36 | motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
37 | }
38 |
39 | public DcMotorEx getMotor() {
40 | return motor;
41 | }
42 |
43 | /**
44 | * Updates the power sent to the motor according to the pidf controller.
45 | */
46 | public void update() {
47 | double change = Range.clip(this.pidfController.update(motor.getVelocity()), -maxPower, maxPower);
48 | power += change;
49 | power = Range.clip(power, -1, 1);
50 | motor.setPower(power);
51 | }
52 |
53 | /**
54 | * Update the PID values in the controller.
55 | * Note that it is not possible to replace f after instantiation
56 | * @param newPID the new pid values to use
57 | */
58 | public void setPIDCoefficients(PIDCoefficients newPID) {
59 | this.pid.kP = newPID.kP;
60 | this.pid.kI = newPID.kI;
61 | this.pid.kD = newPID.kD;
62 | }
63 |
64 | public void setTargetVelocity(int velocity) {
65 | this.targetVelocity = velocity;
66 | this.power = ((double)velocity)/2400;
67 | this.pidfController.setTargetPosition(velocity - internalOffset);
68 | }
69 |
70 | private class TargetVelocityAction implements Action {
71 | int velocity;
72 |
73 | public TargetVelocityAction(int velocity, boolean blocking) {
74 | this.velocity = velocity;
75 | }
76 |
77 | @Override
78 | public boolean run(TelemetryPacket packet) {
79 | Log.d("VELOCITY", String.valueOf(velocity));
80 | setTargetVelocity(velocity);
81 | return false;
82 | }
83 | }
84 |
85 | /**
86 | * Creates an action that will call setTargetVelocity with the provided velocity
87 | *
88 | * @param velocity the desired encoder target velocity
89 | */
90 | public Action setTargetVelocityAction(int velocity) {
91 | return new TargetVelocityAction(velocity, false);
92 | }
93 |
94 | public void zeroMotorInternals() {
95 | motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
96 | motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
97 | }
98 |
99 | public void resetIntegralGain() {
100 | this.pidfController.reset();
101 | }
102 |
103 | /**
104 | * Returns the current target encoder velocity for this motor.
105 | * @return the current target encoder velocity for this motor.
106 | */
107 | public int getTargetVelocity() {
108 | return targetVelocity;
109 | }
110 |
111 | /**
112 | * Returns the current velocity of the motor, in ticks per second
113 | * @return the current velocity of the motor
114 | */
115 | public double getVelocity() {
116 | return motor.getVelocity();
117 | }
118 |
119 | /**
120 | * Sets the maximum power level that can be sent to the motor
121 | * @param maxPower the maximum level of the motor, a value in the interval [0.0, 1.0]
122 | */
123 | public void setMaxPower(double maxPower) {
124 | this.maxPower = Math.abs(maxPower);
125 | }
126 |
127 | /**
128 | * Returns the maximum power level that can be sent to the motor
129 | * @return the maximum level of the motor, a value in the interval [0.0, 1.0]
130 | */
131 | public double getMaxPower() {
132 | return maxPower;
133 | }
134 |
135 | /**
136 | * Returns the current power level sent to the motor.
137 | * @return the current level of the motor, a value in the interval [-1.0, 1.0]
138 | */
139 | public double getPower() {
140 | return motor.getPower();
141 | }
142 |
143 | /**
144 | * Sets the motor power to zero
145 | */
146 | public void stopMotor() {
147 | motor.setPower(0);
148 | }
149 |
150 | /**
151 | * Sets the logical direction in which this motor operates.
152 | * @param direction the direction to set for this motor
153 | */
154 | public void setDirection(DcMotorSimple.Direction direction) {
155 | motor.setDirection(direction);
156 | }
157 | }
158 |
--------------------------------------------------------------------------------
/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java:
--------------------------------------------------------------------------------
1 | package org.firstinspires.ftc.teamcode.tuning;
2 |
3 | import com.acmerobotics.dashboard.FtcDashboard;
4 | import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
5 | import com.acmerobotics.roadrunner.MotorFeedforward;
6 | import com.acmerobotics.roadrunner.Pose2d;
7 | import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
8 | import com.acmerobotics.roadrunner.ftc.DriveType;
9 | import com.acmerobotics.roadrunner.ftc.DriveView;
10 | import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
11 | import com.acmerobotics.roadrunner.ftc.Encoder;
12 | import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
13 | import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
14 | import com.acmerobotics.roadrunner.ftc.LateralPushTest;
15 | import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
16 | import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
17 | import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
18 | import com.qualcomm.hardware.lynx.LynxModule;
19 | import com.qualcomm.robotcore.eventloop.opmode.OpMode;
20 | import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
21 | import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
22 |
23 | import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
24 | import org.firstinspires.ftc.teamcode.MecanumDrive;
25 | import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
26 |
27 | import java.util.ArrayList;
28 | import java.util.Arrays;
29 | import java.util.List;
30 |
31 | public final class TuningOpModes {
32 | public static final Class DRIVE_CLASS = MecanumDrive.class;
33 |
34 | public static final String GROUP = "quickstart";
35 | public static final boolean DISABLED = false;
36 |
37 | private TuningOpModes() {
38 | }
39 |
40 | private static OpModeMeta metaForClass(Class cls) {
41 | return new OpModeMeta.Builder()
42 | .setName(cls.getSimpleName())
43 | .setGroup(GROUP)
44 | .setFlavor(OpModeMeta.Flavor.TELEOP)
45 | .build();
46 | }
47 |
48 | @OpModeRegistrar
49 | public static void register(OpModeManager manager) {
50 | if (DISABLED)
51 | return;
52 |
53 | DriveViewFactory dvf;
54 | dvf = hardwareMap -> {
55 | MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
56 |
57 | List leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
58 | List parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
59 | ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
60 | parEncs.add(dl.par0);
61 | parEncs.add(dl.par1);
62 | perpEncs.add(dl.perp);
63 |
64 | return new DriveView(
65 | DriveType.MECANUM,
66 | MecanumDrive.PARAMS.inPerTick,
67 | MecanumDrive.PARAMS.defaultWheelVel,
68 | MecanumDrive.PARAMS.minProfileAccel,
69 | MecanumDrive.PARAMS.maxProfileAccel,
70 | hardwareMap.getAll(LynxModule.class),
71 | Arrays.asList(
72 | md.leftFront,
73 | md.leftBack),
74 | Arrays.asList(
75 | md.rightFront,
76 | md.rightBack),
77 | leftEncs,
78 | rightEncs,
79 | parEncs,
80 | perpEncs,
81 | md.imu,
82 | md.voltageSensor,
83 | () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
84 | MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
85 | MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick));
86 | };
87 |
88 | manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf));
89 | manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf));
90 | manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf));
91 | manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf));
92 | manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf));
93 | manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf));
94 | manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf));
95 |
96 | manager.register(metaForClass(ManualAxialTuner.class), ManualAxialTuner.class);
97 | manager.register(metaForClass(SplineTest.class), SplineTest.class);
98 | manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class);
99 |
100 | FtcDashboard.getInstance().withConfigRoot(configRoot -> {
101 | for (Class c : Arrays.asList(
102 | AngularRampLogger.class,
103 | ForwardRampLogger.class,
104 | LateralRampLogger.class,
105 | ManualFeedforwardTuner.class,
106 | MecanumMotorDirectionDebugger.class,
107 | ManualAxialTuner.class)) {
108 | configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
109 | }
110 | });
111 | }
112 | }
113 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java:
--------------------------------------------------------------------------------
1 | /*
2 | * Copyright (c) 2023 FIRST
3 | *
4 | * All rights reserved.
5 | *
6 | * Redistribution and use in source and binary forms, with or without modification,
7 | * are permitted (subject to the limitations in the disclaimer below) provided that
8 | * the following conditions are met:
9 | *
10 | * Redistributions of source code must retain the above copyright notice, this list
11 | * of conditions and the following disclaimer.
12 | *
13 | * Redistributions in binary form must reproduce the above copyright notice, this
14 | * list of conditions and the following disclaimer in the documentation and/or
15 | * other materials provided with the distribution.
16 | *
17 | * Neither the name of FIRST nor the names of its contributors may be used to
18 | * endorse or promote products derived from this software without specific prior
19 | * written permission.
20 | *
21 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
22 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
24 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
25 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
26 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
30 | * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
31 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 | */
33 |
34 | package org.firstinspires.ftc.robotcontroller.external.samples;
35 |
36 | import android.util.Size;
37 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
38 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
39 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
40 | import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
41 | import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
42 | import org.firstinspires.ftc.vision.VisionPortal;
43 |
44 | import java.util.Locale;
45 |
46 | /*
47 | * This OpMode helps calibrate a webcam or RC phone camera, useful for AprilTag pose estimation
48 | * with the FTC VisionPortal. It captures a camera frame (image) and stores it on the Robot Controller
49 | * (Control Hub or RC phone), with each press of the gamepad button X (or Square).
50 | * Full calibration instructions are here:
51 | *
52 | * https://ftc-docs.firstinspires.org/camera-calibration
53 | *
54 | * In Android Studio, copy this class into your "teamcode" folder with a new name.
55 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
56 | *
57 | * In OnBot Java, use "Add File" to add this OpMode from the list of Samples.
58 | */
59 |
60 | @TeleOp(name = "Utility: Camera Frame Capture", group = "Utility")
61 | //@Disabled
62 | public class UtilityCameraFrameCapture extends LinearOpMode
63 | {
64 | /*
65 | * EDIT THESE PARAMETERS AS NEEDED
66 | */
67 | final boolean USING_WEBCAM = true;
68 | final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.BACK;
69 | final int RESOLUTION_WIDTH = 1280;
70 | final int RESOLUTION_HEIGHT = 720;
71 |
72 | // Internal state
73 | boolean lastX;
74 | int frameCount;
75 | long capReqTime;
76 |
77 | @Override
78 | public void runOpMode()
79 | {
80 | VisionPortal portal;
81 |
82 | if (USING_WEBCAM)
83 | {
84 | portal = new VisionPortal.Builder()
85 | .setCamera(hardwareMap.get(WebcamName.class, "WebcamOuttake"))
86 | .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
87 | .build();
88 | }
89 | else
90 | {
91 | portal = new VisionPortal.Builder()
92 | .setCamera(INTERNAL_CAM_DIR)
93 | .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
94 | .build();
95 | }
96 |
97 | while (!isStopRequested())
98 | {
99 | boolean x = gamepad1.x;
100 |
101 | if (x && !lastX)
102 | {
103 | portal.saveNextFrameRaw(String.format(Locale.US, "CameraFrameCapture-%06d", frameCount++));
104 | capReqTime = System.currentTimeMillis();
105 | }
106 |
107 | lastX = x;
108 |
109 | telemetry.addLine("######## Camera Capture Utility ########");
110 | telemetry.addLine(String.format(Locale.US, " > Resolution: %dx%d", RESOLUTION_WIDTH, RESOLUTION_HEIGHT));
111 | telemetry.addLine(" > Press X (or Square) to capture a frame");
112 | telemetry.addData(" > Camera Status", portal.getCameraState());
113 |
114 | if (capReqTime != 0)
115 | {
116 | telemetry.addLine("\nCaptured Frame!");
117 | }
118 |
119 | if (capReqTime != 0 && System.currentTimeMillis() - capReqTime > 1000)
120 | {
121 | capReqTime = 0;
122 | }
123 |
124 | telemetry.update();
125 | }
126 | }
127 | }
128 |
--------------------------------------------------------------------------------
/.github/CONTRIBUTING.md:
--------------------------------------------------------------------------------
1 | # Contributing to the FTC SDK
2 |
3 | The following is a set of guidelines for contributing the FIRST FTC SDK. The FTC Technology Team welcomes suggestions for improvements to core software, ideas for new features, requests for built-in support of new sensors, and well written bug reports.
4 |
5 | ## How can I contribute?
6 |
7 | ### Pull requests
8 |
9 | __STOP!__ If you are new to git, do not understand the mechanics of forks, branches, and pulls, if what you just read is confusing, __do not__ push this button. Most likely it won't do what you think it will.
10 |
11 | 
12 |
13 | If you are looking at this button then you've pushed some changes to your team's fork of ftctechnh/ftc_app. Congratulations! You are almost certainly finished.
14 |
15 | The vast majority of pull requests seen on the ftctechnh/ftc_app repository are not intended to be merged into the official SDK. Team software is just that, your team's. It's specific to the tasks you are trying to accomplish, the testing you are doing, and goals your team has. You don't want that pushed into the official SDK.
16 |
17 | If what you've read so far makes little sense, there are some very good git learning resources online.
18 | [Git Book](https://git-scm.com/book/en/v2)
19 | [Interactive Git Tutorial](https://try.github.io)
20 |
21 | ### Guidlines for experienced GIT users.
22 |
23 | If you are absolutely certain that you want to push the big green button above, read on. Otherwise back _slowly away from keyboard_.
24 |
25 | The real intent for advanced users is often to issue a pull request from the [branch](https://www.atlassian.com/git/tutorials/using-branches/git-branch) on a local fork back to master on either the same local fork or a child of the team fork and not on the parent ftctechnh/ftc_app. See [Creating a Pull Request](https://help.github.com/articles/creating-a-pull-request-from-a-fork/).
26 |
27 | If that is indeed the intent, then you can merge your [topic branch](https://git-scm.com/book/en/v2/Git-Branching-Branching-Workflows#Topic-Branches) into master locally by hand before pushing it up to github, or if you want a pull request for pulls between branches on the same repository because, say, you want team members to look at your software before merging into master, you can select the base fork from the dropdown on the "Open a pull request" page and select your team repo instead of ftctechnh's.
28 |
29 | Alternatively, if you have a team repository forked from ftctechnh/ftc_app, and then team members individually fork from your team repository, then pull requests from the individual team member's forks will have the main team repository automatically selected as the base fork for the pull. And you won't inadvertently request to pull your team software into ftctechnh's repository.
30 |
31 | The latter would be the "best" way to manage software among a large team. But as with all things git there are many options.
32 |
33 | Pull requests that do not fall into the category above are evaluated by the FTC Technology Team on a case-by-case basis. Please note however that the deployment model of the SDK does not support direct pulls into ftctechnh/ftc_app.
34 |
35 | ### Report bugs
36 |
37 | This section guides you through filing a bug report. The better the report the more likely it is to be root caused and fixed. Please refrain from feature requests or software enhancements when opening new issues. See Suggesting Enhancements below.
38 |
39 | #### Before submitting a bug report
40 |
41 | - Check the [forums](http://ftcforum.firstinspires.org/forum.php) to see if someone else has run into the problem and whether there is an official solution that doesn't require a new SDK.
42 |
43 | - Perform a search of current [issues](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues) to see if the problem has already been reported. If so, add a comment to the existing issue instead of creating a new one.
44 |
45 | #### How Do I Submit A (Good) Bug Report?
46 |
47 | Bugs are tracked as GitHub issues. Create an issue on ftctechnh/ftc_app and provide the following information.
48 | Explain the problem and include additional details to help maintainers reproduce the problem:
49 |
50 | - Use a clear and descriptive title for the issue to identify the problem.
51 |
52 | - Describe the exact steps which reproduce the problem in as many details as possible.
53 |
54 | - Provide specific examples to demonstrate the steps.
55 |
56 | - Describe the behavior you observed after following the steps and point out what exactly is the problem with that behavior. Explain which behavior you expected to see instead and why. If applicable, include screenshots which show you following the described steps and clearly demonstrate the problem.
57 |
58 | - If you're reporting that the RobotController crashed, include the logfile with a stack trace of the crash. [Example of good bug report with stack trace](https://github.com/ftctechnh/ftc_app/issues/224)
59 |
60 | - If the problem wasn't triggered by a specific action, describe what you were doing before the problem happened and share more information using the guidelines below.
61 |
62 | ### Suggesting Enhancements
63 |
64 | FIRST volunteers are awesome. You all have great ideas and we want to hear them.
65 |
66 | Enhancements should be broadly applicable to a large majority of teams, should not force teams to change their workflow, and should provide real value to the mission of FIRST as it relates to engaging youth in engineering activities.
67 |
68 | The best way to get momentum behind new features is to post a description of your idea in the discussions section of this repository. Build community support for it. The FTC Technology Team monitors the discussions. We'll hear you and if there's a large enough call for the feature it's very likely to get put on the list for a future release.
69 |
--------------------------------------------------------------------------------
/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 | * Error for attempting an illegal remapping (lhs or multiple same axes)
13 | */
14 | public static class InvalidAxisRemapException extends RuntimeException {
15 | public InvalidAxisRemapException(String detailMessage) {
16 | super(detailMessage);
17 | }
18 | }
19 |
20 | /**
21 | * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
22 | * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
23 | * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
24 | *
25 | * Adapted from this post.
26 | * Reference the BNO055 Datasheet for details.
27 | *
28 | * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
29 | *
30 | * @param imu IMU
31 | * @param order axes order
32 | * @param signs axes signs
33 | */
34 | public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
35 | try {
36 | // the indices correspond with the 2-bit axis encodings specified in the datasheet
37 | int[] indices = order.indices();
38 | // AxesSign's values align with the datasheet
39 | int axisMapSigns = signs.bVal;
40 |
41 | if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
42 | throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
43 | }
44 |
45 | // ensure right-handed coordinate system
46 | boolean isXSwapped = indices[0] != 0;
47 | boolean isYSwapped = indices[1] != 1;
48 | boolean isZSwapped = indices[2] != 2;
49 | boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
50 | && (!isXSwapped || !isYSwapped || !isZSwapped);
51 | boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
52 | // != functions as xor
53 | if (areTwoAxesSwapped != oddNumOfFlips) {
54 | throw new InvalidAxisRemapException("Coordinate system is left-handed");
55 | }
56 |
57 | // Bit: 7 6 | 5 4 | 3 2 | 1 0 |
58 | // reserved | z axis | y axis | x axis |
59 | int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
60 |
61 | // Enter CONFIG mode
62 | imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
63 |
64 | Thread.sleep(100);
65 |
66 | // Write the AXIS_MAP_CONFIG register
67 | imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
68 |
69 | // Write the AXIS_MAP_SIGN register
70 | imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
71 |
72 | // Switch back to the previous mode
73 | imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
74 |
75 | Thread.sleep(100);
76 | } catch (InterruptedException e) {
77 | Thread.currentThread().interrupt();
78 | }
79 | }
80 |
81 | /**
82 | * Remaps the IMU coordinate system so that the remapped +Z faces the provided
83 | * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
84 | *
85 | * @param imu IMU
86 | * @param direction axis direction
87 | */
88 | public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
89 | switch (direction) {
90 | case POS_X:
91 | swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
92 | break;
93 | case NEG_X:
94 | swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
95 | break;
96 | case POS_Y:
97 | swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
98 | break;
99 | case NEG_Y:
100 | swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
101 | break;
102 | case POS_Z:
103 | swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
104 | break;
105 | case NEG_Z:
106 | swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
107 | break;
108 | }
109 | }
110 |
111 | /**
112 | * Now deprecated due to unintuitive parameter order.
113 | * Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
114 | *
115 | * @param imu IMU
116 | * @param order axes order
117 | * @param signs axes signs
118 | */
119 | @Deprecated
120 | public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
121 | AxesOrder adjustedAxesOrder = order.reverse();
122 | int[] indices = order.indices();
123 | int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
124 | AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
125 |
126 | swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
127 | }
128 | }
129 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java:
--------------------------------------------------------------------------------
1 | /* Copyright (c) 2018 FIRST. All rights reserved.
2 | *
3 | * Redistribution and use in source and binary forms, with or without modification,
4 | * are permitted (subject to the limitations in the disclaimer below) provided that
5 | * the following conditions are met:
6 | *
7 | * Redistributions of source code must retain the above copyright notice, this list
8 | * of conditions and the following disclaimer.
9 | *
10 | * Redistributions in binary form must reproduce the above copyright notice, this
11 | * list of conditions and the following disclaimer in the documentation and/or
12 | * other materials provided with the distribution.
13 | *
14 | * Neither the name of FIRST nor the names of its contributors may be used to endorse or
15 | * promote products derived from this software without specific prior written permission.
16 | *
17 | * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
18 | * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
20 | * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 | */
29 |
30 | package org.firstinspires.ftc.robotcontroller.external.samples;
31 |
32 | import com.qualcomm.robotcore.eventloop.opmode.Disabled;
33 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
34 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
35 | import com.qualcomm.robotcore.hardware.DcMotorSimple;
36 | import com.qualcomm.robotcore.util.ElapsedTime;
37 | import com.qualcomm.robotcore.util.Range;
38 |
39 |
40 | /*
41 | * This OpMode executes a basic Tank Drive Teleop for a two wheeled robot using two REV SPARKminis.
42 | * To use this example, connect two REV SPARKminis into servo ports on the Expansion Hub. On the
43 | * robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller'
44 | * and name them 'left_drive' and 'right_drive'.
45 | *
46 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
47 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
48 | */
49 |
50 | @TeleOp(name="REV SPARKmini Simple Drive Example", group="Concept")
51 | @Disabled
52 | public class ConceptRevSPARKMini extends LinearOpMode {
53 |
54 | // Declare OpMode members.
55 | private ElapsedTime runtime = new ElapsedTime();
56 | private DcMotorSimple leftDrive = null;
57 | private DcMotorSimple rightDrive = null;
58 |
59 | @Override
60 | public void runOpMode() {
61 | telemetry.addData("Status", "Initialized");
62 | telemetry.update();
63 |
64 | // Initialize the hardware variables. Note that the strings used here as parameters
65 | // to 'get' must correspond to the names assigned during the robot configuration
66 | // step (using the FTC Robot Controller app on the phone).
67 | leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive");
68 | rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive");
69 |
70 | // Most robots need the motor on one side to be reversed to drive forward
71 | // Reverse the motor that runs backward when connected directly to the battery
72 | leftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
73 | rightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
74 |
75 | // Wait for the game to start (driver presses PLAY)
76 | waitForStart();
77 | runtime.reset();
78 |
79 | // run until the end of the match (driver presses STOP)
80 | while (opModeIsActive()) {
81 |
82 | // Setup a variable for each drive wheel to save power level for telemetry
83 | double leftPower;
84 | double rightPower;
85 |
86 | // Choose to drive using either Tank Mode, or POV Mode
87 | // Comment out the method that's not used. The default below is POV.
88 |
89 | // POV Mode uses left stick to go forward, and right stick to turn.
90 | // - This uses basic math to combine motions and is easier to drive straight.
91 | double drive = -gamepad1.left_stick_y;
92 | double turn = gamepad1.right_stick_x;
93 | leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
94 | rightPower = Range.clip(drive - turn, -1.0, 1.0) ;
95 |
96 | // Tank Mode uses one stick to control each wheel.
97 | // - This requires no math, but it is hard to drive forward slowly and keep straight.
98 | // leftPower = -gamepad1.left_stick_y ;
99 | // rightPower = -gamepad1.right_stick_y ;
100 |
101 | // Send calculated power to wheels
102 | leftDrive.setPower(leftPower);
103 | rightDrive.setPower(rightPower);
104 |
105 | // Show the elapsed game time and wheel power.
106 | telemetry.addData("Status", "Run Time: " + runtime.toString());
107 | telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
108 | telemetry.update();
109 | }
110 | }
111 | }
112 |
--------------------------------------------------------------------------------