Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions FtcRobotController/src/main/AndroidManifest.xml
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:tools="http://schemas.android.com/tools"
android:versionCode="57"
android:versionName="10.1.1">
android:versionCode="60"
android:versionName="11.0">

<uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,20 +69,20 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {

// Declare OpMode members for each of the 4 motors.
private ElapsedTime runtime = new ElapsedTime();
private DcMotor leftFrontDrive = null;
private DcMotor leftBackDrive = null;
private DcMotor rightFrontDrive = null;
private DcMotor rightBackDrive = null;
private DcMotor frontLeftDrive = null;
private DcMotor backLeftDrive = null;
private DcMotor frontRightDrive = null;
private DcMotor backRightDrive = null;

@Override
public void runOpMode() {

// Initialize the hardware variables. Note that the strings used here must correspond
// to the names assigned during the robot configuration step on the DS or RC devices.
leftFrontDrive = hardwareMap.get(DcMotor.class, "left_front_drive");
leftBackDrive = hardwareMap.get(DcMotor.class, "left_back_drive");
rightFrontDrive = hardwareMap.get(DcMotor.class, "right_front_drive");
rightBackDrive = hardwareMap.get(DcMotor.class, "right_back_drive");
frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive");
backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive");
frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive");
backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive");

// ########################################################################################
// !!! IMPORTANT Drive Information. Test your motor directions. !!!!!
Expand All @@ -94,10 +94,10 @@ public void runOpMode() {
// when you first test your robot, push the left joystick forward and observe the direction the wheels turn.
// Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward
// Keep testing until ALL the wheels move the robot forward when you push the left joystick forward.
leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
frontRightDrive.setDirection(DcMotor.Direction.FORWARD);
backRightDrive.setDirection(DcMotor.Direction.FORWARD);

// Wait for the game to start (driver presses START)
telemetry.addData("Status", "Initialized");
Expand All @@ -117,22 +117,22 @@ public void runOpMode() {

// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
double frontLeftPower = axial + lateral + yaw;
double frontRightPower = axial - lateral - yaw;
double backLeftPower = axial - lateral + yaw;
double backRightPower = axial + lateral - yaw;

// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower));
max = Math.max(max, Math.abs(backLeftPower));
max = Math.max(max, Math.abs(backRightPower));

if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
frontLeftPower /= max;
frontRightPower /= max;
backLeftPower /= max;
backRightPower /= max;
}

// This is test code:
Expand All @@ -146,22 +146,22 @@ public void runOpMode() {
// Once the correct motors move in the correct direction re-comment this code.

/*
leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
frontLeftPower = gamepad1.x ? 1.0 : 0.0; // X gamepad
backLeftPower = gamepad1.a ? 1.0 : 0.0; // A gamepad
frontRightPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad
backRightPower = gamepad1.b ? 1.0 : 0.0; // B gamepad
*/

// Send calculated power to wheels
leftFrontDrive.setPower(leftFrontPower);
rightFrontDrive.setPower(rightFrontPower);
leftBackDrive.setPower(leftBackPower);
rightBackDrive.setPower(rightBackPower);
frontLeftDrive.setPower(frontLeftPower);
frontRightDrive.setPower(frontRightPower);
backLeftDrive.setPower(backLeftPower);
backRightDrive.setPower(backRightPower);

// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
telemetry.addData("Front left/Right", "%4.2f, %4.2f", frontLeftPower, frontRightPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", backLeftPower, backRightPower);
telemetry.update();
}
}}
Original file line number Diff line number Diff line change
Expand Up @@ -224,14 +224,17 @@ private void telemetryAprilTag() {
for (AprilTagDetection detection : currentDetections) {
if (detection.metadata != null) {
telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)",
detection.robotPose.getPosition().x,
detection.robotPose.getPosition().y,
detection.robotPose.getPosition().z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)",
detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES)));
// Only use tags that don't have Obelisk in them
if (!detection.metadata.name.contains("Obelisk")) {
telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)",
detection.robotPose.getPosition().x,
detection.robotPose.getPosition().y,
detection.robotPose.getPosition().z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)",
detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES),
detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES)));
}
} else {
telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y));
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
/* Copyright (c) 2025 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

package org.firstinspires.ftc.robotcontroller.external.samples;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

/*
* Demonstrates how to use an OpMode to store data in the blackboard and retrieve it.
* This is useful for storing information in Auto and then retrieving it in your TeleOp.
*
* Be aware that this is NOT saved across power reboots or downloads or robot resets.
*
* You also need to make sure that both the storing and retrieving are done using the same
* type. For example, storing a Double in the blackboard and retrieving it as an Integer
* will fail when you typecast it.
*
* The blackboard is a standard hashmap so you can use methods like:
* put, get, containsKey, remove, etc.
*/
@TeleOp(name = "Concept: Blackboard", group = "Concept")
@Disabled
public class ConceptBlackboard extends OpMode {
// We use constants here so we won't have the problem of typos in different places when we
// meant to refer to the same thing.
public static final String TIMES_STARTED_KEY = "Times started";
public static final String ALLIANCE_KEY = "Alliance";

/**
* This method will be called once, when the INIT button is pressed.
*/
@Override
public void init() {
// This gets us what is in the blackboard or the default if it isn't in there.
Object timesStarted = blackboard.getOrDefault(TIMES_STARTED_KEY, 0);
blackboard.put(TIMES_STARTED_KEY, (int) timesStarted + 1);

telemetry.addData("OpMode started times", blackboard.get(TIMES_STARTED_KEY));
}

/**
* This method will be called repeatedly during the period between when
* the START button is pressed and when the OpMode is stopped.
* <p>
* If the left bumper is pressed it will store the value "RED" in the blackboard for alliance.
* If the right bumper is pressed it will store the value "BLUE" in the blackboard for alliance.
* <p>
* You'll notice that if you quit the OpMode and come back in, the value will persist.
*/
@Override
public void loop() {
if (gamepad1.left_bumper) {
blackboard.put(ALLIANCE_KEY, "RED");
} else if (gamepad1.right_bumper) {
blackboard.put(ALLIANCE_KEY, "BLUE");
}
telemetry.addData("Alliance", blackboard.get(ALLIANCE_KEY));
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
/*
Copyright (c) 2024 Miriam Sinton-Remes

All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted (subject to the limitations in the disclaimer below) provided that
the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.

Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

Neither the name of FIRST nor the names of its contributors may be used to
endorse or promote products derived from this software without specific prior
written permission.

NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

package org.firstinspires.ftc.robotcontroller.external.samples;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

/*
* This OpMode illustrates using edge detection on a gamepad.
*
* Simply checking the state of a gamepad button each time could result in triggering an effect
* multiple times. Edge detection ensures that you only detect one button press, regardless of how
* long the button is held.
*
* There are two main types of edge detection. Rising edge detection will trigger when a button is
* first pressed. Falling edge detection will trigger when the button is released.
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*/

@Disabled
@TeleOp(name="Concept: Gamepad Edge Detection", group ="Concept")
public class ConceptGamepadEdgeDetection extends LinearOpMode {

@Override
public void runOpMode() {
// Wait for the DS start button to be pressed
waitForStart();

while (opModeIsActive()) {
// Update the telemetry
telemetryButtonData();

// Wait 2 seconds before doing another check
sleep(2000);
}
}

public void telemetryButtonData() {
// Add the status of the Gamepad 1 Left Bumper
telemetry.addData("Gamepad 1 Left Bumper Pressed", gamepad1.leftBumperWasPressed());
telemetry.addData("Gamepad 1 Left Bumper Released", gamepad1.leftBumperWasReleased());
telemetry.addData("Gamepad 1 Left Bumper Status", gamepad1.left_bumper);

// Add an empty line to separate the buttons in telemetry
telemetry.addLine();

// Add the status of the Gamepad 1 Right Bumper
telemetry.addData("Gamepad 1 Right Bumper Pressed", gamepad1.rightBumperWasPressed());
telemetry.addData("Gamepad 1 Right Bumper Released", gamepad1.rightBumperWasReleased());
telemetry.addData("Gamepad 1 Right Bumper Status", gamepad1.right_bumper);

// Add a note that the telemetry is only updated every 2 seconds
telemetry.addLine("\nTelemetry is updated every 2 seconds.");

// Update the telemetry on the DS screen
telemetry.update();
}
}
Loading