# Welcome to the OnBotJava Code Editor

If you are just starting out, click the '+' (Add File) icon in the top left corner.
Enter your new file name, and then choose one of the many samples.
If you just want to drive a basic robot, select the "BasicOpMode_Linear" sample.
Select the "TeleOp" radio button, and then click "OK".

The sample you chose will be renamed to match the name you entered, and it
will appear on the "project files" list in the left pane.

To edit your code, just click on the desired file in the left hand pane,
and it will be loaded into this Code Editor window. Make any changes. You can also
use Ctrl-F (or Cmd-F on Macs) to search the file you are working on.

Once you are done, click the "Build Everything" icon at the bottom of this pane.
This will build your OpModes and report any errors.
If there are no errors, the OpModes will be stored on the Robot for immediate use.

## Samples

There are a range of different samples to choose from.

Sample names use a convention which helps to indicate their general, and specific,
purpose.

eg: The name's prefix describes the general purpose, which can be one of the
following:

* Basic: This is a minimally functional OpMode used to illustrate the

skeleton\/structure
of a particular style of OpMode. These are bare bones examples.
* Sensor: This is a Sample OpMode that shows how to use a specific sensor.
It is not intended as a functioning robot, it is simply showing the
minimal code
required to read and display the sensor values.
* Robot: This is a Sample OpMode that assumes a simple two-motor (differential)
drive base.
It may be used to provide a common baseline driving OpMode, or
to demonstrate how a particular sensor or concept can be used to
navigate.
* Concept: This is a sample OpMode that illustrates performing a specific function
or concept.
These may be complex, but their operation will be explained clearly in
the comments,
or the header should reference an external doc., guide or tutorial.

For more help, visit the FTC Control System Wiki (https://github.com/FIRST-Tech-
Challenge/FtcRobotController/wiki)

Please report any encountered issues on [GitHub](https://github.com/FIRST-Tech-

Challenge/FtcRobotController).

## What's Java 8?

Java 8 provides more ways to help you write more concise, readable, and
maintainable code
for your robot.

We're adding Java 8 editor support to this SDK release.

To enable support, you can enable `Enable beta Java 8 editor features` in the
Settings menu.

For an example, take the following code snippet from

the `ConceptTelemetry`[0] sample you might have already seen in prior years

```
/**
* As an illustration, the first line on our telemetry display will display the
* battery voltage. The idea here is that it's expensive to compute the voltage
* (at least for purposes of illustration) so you don't want to do it unless the
* data is <em>actually</em> going to make it to the driver station (recall that
* telemetry transmission is throttled to reduce bandwidth use.
* Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage
* sensor attached.
*
* @see Telemetry#getMsTransmissionInterval()
*/
telemetry.addData("voltage", "%.1f volts", new Func<Double>() {
@Override public Double value() {
return getBatteryVoltage();
}
});
```

There's a few ways to rewrite the same snippet to be more concise by using
what's called a lambda (lamb-da) expression[1] from the new Java 8 editor support.

The part of the code that simply defines what's called an "anonymous class"
can be actually rewritten as a lambda expression.

The anonymous class above is the following section:

```
new Func<Double>() {
@Override public Double value() {
return getBatteryVoltage();
}
}
```

What you actually care about is the return value (`getBatteryVoltage()`)

of the function you defined, not the wrapping syntax (`new Func<Double>{...}`).

As a general pattern, for a function that takes no arguments, you can remove
the class declaration, method return type, and method name and the compiler
will get what you are implying.

To see this visually, this is the rewritten lambda expression:

```
() -> {
// this area wrapped by the curly braces is the lambda body
return getBatteryVoltage();
}
```

This leaves the lambda body intact and is a useful pattern if your return value
needs a multiline calculation.

In this case, we only have the method return taking a single line, so we can
"unwrap" the return expression:
```
() -> getBatteryVoltage()
```

This again leaves a bit of room to make more concise, applying the same pattern,
what can we remove?

```
this::getBatteryVoltage
```

This is called a method reference, which requires only two things, the object and
the method you want to call on the object,
Advanced note: the `this` comes from the `this` that was implied indirectly in the
anonymous class
usage through a capture, but now you actually need to add this `this` reference.

Put all together, the same snippet can be rewritten simply as:
```
telemetry.addData("voltage", "%.1f volts", this::getBatteryVoltage);
```

We're still bound by some limitations of the Android platform, you can check
https://developer.android.com/studio/write/java8-support-table for known
limitations for more advanced features.

### Known limitations

- Android 6.0 Marshmallow devices cannot use Java 8 features
- `IntStream` usage via direct usage can cause Robot Controller crashes.
- You can use `Arrays.of(new int[]{1})` as a workaround.

0:
https://github.com/FIRST-Tech-Challenge/FtcRobotController/blob/00cbf344526d991ce33
452437018af9f119d22d1/FtcRobotController/src/main/java/org/firstinspires/ftc/
robotcontroller/external/samples/ConceptTelemetry.java#L110-L123
1: https://docs.oracle.com/javase/tutorial/java/javaOO/lambdaexpressions.html
2:
https://docs.oracle.com/javase/tutorial/java/javaOO/anonymousclasses.html#accessing

/* Copyright (c) 2021 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.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

/*
* This file contains an example of a Linear "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period
of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or
Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-
design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from
above.
*
* Also note that it is critical to set the correct rotation direction for each
motor. See details below.
*
* Holonomic drives provide the ability for the robot to move in three axes
(directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* 1) Axial: Driving forward and backward Left-joystick
Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and
Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right
and Left
*
* This code is written assuming that the right-side motors need to be reversed for
the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left
stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* 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
*/

@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode")

public class MyBasicOmniOpMode_Linear extends LinearOpMode {

// Declare OpMode members for each of the 4 motors.

private ElapsedTime runtime = new ElapsedTime();
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.
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.
!!!!!
//
###################################################################################
#####
// Most robots need the motors on one side to be reversed to drive forward.
// The motor reversals shown here are for a "direct drive" robot (the
wheels turn the same direction as the motor shaft)
// If your robot has additional gear reductions or uses a right-angled
drive, it's important to ensure
// that your motors are turning in the correct direction. So, start out
with the reversals here, BUT
// 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.
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");
telemetry.update();

waitForStart();
 runtime.reset();

// run until the end of the match (driver presses STOP)

while (opModeIsActive()) {
double max;

// POV Mode uses left joystick to go forward & strafe, and right
joystick to rotate.
double axial = -gamepad1.left_stick_y; // Note: pushing stick
forward gives negative value
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;

// 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 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(frontLeftPower), Math.abs(frontRightPower));
max = Math.max(max, Math.abs(backLeftPower));
max = Math.max(max, Math.abs(backRightPower));

if (max > 1.0) {

frontLeftPower /= max;
frontRightPower /= max;
backLeftPower /= max;
backRightPower /= max;
}

// This is test code:

//
// Uncomment the following code to test your motor directions.
// Each button should make the corresponding motor run FORWARD.
// 1) First get all the motors to take to correct positions on the
robot
// by adjusting your Robot Configuration if necessary.
// 2) Then make sure they run in the correct direction by modifying
the
// the setDirection() calls above.
// Once the correct motors move in the correct direction re-comment
this code.

/*
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

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", frontLeftPower,
frontRightPower);
telemetry.addData("Back left/Right", "%4.2f, %4.2f", backLeftPower,
backRightPower);
telemetry.update();
}
}}

/* Copyright (c) 2017 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.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

/*
* This file contains an example of an iterative (Non-Linear) "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period
of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode
* class is instantiated on the Robot Controller and executed.
 *
* This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled
robot
* It includes all the skeletal structure that all iterative OpModes contain.
*
* 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
*/

@TeleOp(name="Basic: Iterative OpMode", group="Iterative OpMode")

public class MyBasicOpMode_Iterative extends OpMode

{
// Declare OpMode members.
private ElapsedTime runtime = new ElapsedTime();
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;

/*
* Code to run ONCE when the driver hits INIT
*/
@Override
public void init() {
telemetry.addData("Status", "Initialized");

// Initialize the hardware variables. Note that the strings used here as
parameters
// to 'get' must correspond to the names assigned during the robot
configuration
// step (using the FTC Robot Controller app on the phone).
leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = hardwareMap.get(DcMotor.class, "right_drive");

// To drive forward, most robots need the motor on one side to be reversed,
because the axles point in opposite directions.
// Pushing the left stick forward MUST make robot go forward. So adjust
these two lines based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels.
Gear Reduction or 90 Deg drives may require direction flips
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);

// Tell the driver that initialization is complete.

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

/*
* Code to run REPEATEDLY after the driver hits INIT, but before they hit START
*/
@Override
public void init_loop() {
}

/*
* Code to run ONCE when the driver hits START
*/
@Override
 public void start() {
runtime.reset();
}

/*
* Code to run REPEATEDLY after the driver hits START but before they hit STOP
*/
@Override
public void loop() {
// Setup a variable for each drive wheel to save power level for telemetry
double leftPower;
double rightPower;

// Choose to drive using either Tank Mode, or POV Mode

// Comment out the method that's not used. The default below is POV.

// POV Mode uses left stick to go forward, and right stick to turn.
// - This uses basic math to combine motions and is easier to drive
straight.
double drive = -gamepad1.left_stick_y;
double turn = gamepad1.right_stick_x;
leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
rightPower = Range.clip(drive - turn, -1.0, 1.0) ;

// Tank Mode uses one stick to control each wheel.

// - This requires no math, but it is hard to drive forward slowly and keep
straight.
// leftPower = -gamepad1.left_stick_y ;
// rightPower = -gamepad1.right_stick_y ;

// Send calculated power to wheels

leftDrive.setPower(leftPower);
rightDrive.setPower(rightPower);

// Show the elapsed game time and wheel power.

telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower,
rightPower);
}

/*
* Code to run ONCE after the driver hits STOP
*/
@Override
public void stop() {
}

/* Copyright (c) 2017 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.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;

/*
* This file contains an minimal example of a Linear "OpMode". An OpMode is a
'program' that runs in either
* the autonomous or the teleop period of an FTC match. The names of OpModes appear
on the menu
* of the FTC Driver Station. When a selection is made from the menu, the
corresponding OpMode
* class is instantiated on the Robot Controller and executed.
*
* This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled
robot
* It includes all the skeletal structure that all linear OpModes contain.
*
* 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
*/

@TeleOp(name="Basic: Linear OpMode", group="Linear OpMode")

public class MyBasicOpMode_Linear extends LinearOpMode {

// Declare OpMode members.

private ElapsedTime runtime = new ElapsedTime();
private DcMotor leftDrive = null;
private DcMotor rightDrive = null;
 @Override
public void runOpMode() {
telemetry.addData("Status", "Initialized");
telemetry.update();

// Initialize the hardware variables. Note that the strings used here as
parameters
// to 'get' must correspond to the names assigned during the robot
configuration
// step (using the FTC Robot Controller app on the phone).
leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
rightDrive = hardwareMap.get(DcMotor.class, "right_drive");

// To drive forward, most robots need the motor on one side to be reversed,
because the axles point in opposite directions.
// Pushing the left stick forward MUST make robot go forward. So adjust
these two lines based on your first test drive.
// Note: The settings here assume direct drive on left and right wheels.
Gear Reduction or 90 Deg drives may require direction flips
leftDrive.setDirection(DcMotor.Direction.REVERSE);
rightDrive.setDirection(DcMotor.Direction.FORWARD);

// Wait for the game to start (driver presses START)

waitForStart();
runtime.reset();

// run until the end of the match (driver presses STOP)

while (opModeIsActive()) {

// Setup a variable for each drive wheel to save power level for
telemetry
double leftPower;
double rightPower;

// Choose to drive using either Tank Mode, or POV Mode

// Comment out the method that's not used. The default below is POV.

// POV Mode uses left stick to go forward, and right stick to turn.
// - This uses basic math to combine motions and is easier to drive
straight.
double drive = -gamepad1.left_stick_y;
double turn = gamepad1.right_stick_x;
leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
rightPower = Range.clip(drive - turn, -1.0, 1.0) ;

// Tank Mode uses one stick to control each wheel.

// - This requires no math, but it is hard to drive forward slowly and
keep straight.
// leftPower = -gamepad1.left_stick_y ;
// rightPower = -gamepad1.right_stick_y ;

// Send calculated power to wheels

leftDrive.setPower(leftPower);
rightDrive.setPower(rightPower);

// Show the elapsed game time and wheel power.

telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower,
rightPower);
 telemetry.update();
}
}
}

/* Copyright (c) 2023 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.teamcode;

import android.util.Size;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import
org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;

import java.util.List;

/*
* This OpMode illustrates the basics of AprilTag recognition and pose estimation,
* including Java Builder structures for specifying Vision parameters.
*
* For an introduction to AprilTags, see the FTC-DOCS link below:
* https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/
apriltag_intro/apriltag-intro.html
*
 * In this sample, any visible tag ID will be detected and displayed, but only tags
that are included in the default
* "TagLibrary" will have their position and orientation information displayed.
This default TagLibrary contains
* the current Season's AprilTags and a small set of "test Tags" in the high number
range.
*
* When an AprilTag in the TagLibrary is detected, the SDK provides location and
orientation of the tag, relative to the camera.
* This information is provided in the "ftcPose" member of the returned
"detection", and is explained in the ftc-docs page linked below.
* https://ftc-docs.firstinspires.org/apriltag-detection-values
*
* To experiment with using AprilTags to navigate, try out these two driving
samples:
* RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank
*
* There are many "default" VisionPortal and AprilTag configuration parameters that
may be overridden if desired.
* These default parameters are shown as comments in the code below.
*
* 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.
*/
@TeleOp(name = "Concept: AprilTag", group = "Concept")

public class MyConceptAprilTag extends LinearOpMode {

private static final boolean USE_WEBCAM = true; // true for webcam, false for
phone camera

/**
* The variable to store our instance of the AprilTag processor.
*/
private AprilTagProcessor aprilTag;

/**
* The variable to store our instance of the vision portal.
*/
private VisionPortal visionPortal;

@Override
public void runOpMode() {

initAprilTag();

// Wait for the DS start button to be touched.

telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
telemetry.addData(">", "Touch START to start OpMode");
telemetry.update();
waitForStart();

if (opModeIsActive()) {
while (opModeIsActive()) {

telemetryAprilTag();
 // Push telemetry to the Driver Station.
telemetry.update();

// Save CPU resources; can resume streaming when needed.

if (gamepad1.dpad_down) {
visionPortal.stopStreaming();
} else if (gamepad1.dpad_up) {
visionPortal.resumeStreaming();
}

// Share the CPU.

sleep(20);
}
}

// Save more CPU resources when camera is no longer needed.

visionPortal.close();

} // end method runOpMode()

/**
* Initialize the AprilTag processor.
*/
private void initAprilTag() {

// Create the AprilTag processor.

aprilTag = new AprilTagProcessor.Builder()

// The following default settings are available to un-comment and edit

as needed.
//.setDrawAxes(false)
//.setDrawCubeProjection(false)
//.setDrawTagOutline(true)
//.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
//.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
//.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)

// == CAMERA CALIBRATION ==
// If you do not manually specify calibration parameters, the SDK will
attempt
// to load a predefined calibration for your camera.
//.setLensIntrinsics(578.272, 578.272, 402.145, 221.506)
// ... these parameters are fx, fy, cx, cy.

.build();

// Adjust Image Decimation to trade-off detection-range for detection-rate.

// eg: Some typical detection data using a Logitech C920 WebCam
// Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per
second
// Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per
second
// Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per
Second (default)
// Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per
Second (default)
// Note: Decimation can be changed on-the-fly to adapt during a match.
//aprilTag.setDecimation(3);
 // Create the vision portal by using a builder.
VisionPortal.Builder builder = new VisionPortal.Builder();

// Set the camera (webcam vs. built-in RC phone camera).

if (USE_WEBCAM) {
builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
} else {
builder.setCamera(BuiltinCameraDirection.BACK);
}

// Choose a camera resolution. Not all cameras support all resolutions.

//builder.setCameraResolution(new Size(640, 480));

// Enable the RC preview (LiveView). Set "false" to omit camera

monitoring.
//builder.enableLiveView(true);

// Set the stream format; MJPEG uses less bandwidth than default YUY2.
//builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);

// Choose whether or not LiveView stops if no processors are enabled.

// If set "true", monitor shows solid orange screen if no processors
enabled.
// If set "false", monitor shows camera view without annotations.
//builder.setAutoStopLiveView(false);

// Set and enable the processor.

builder.addProcessor(aprilTag);

// Build the Vision Portal, using the above settings.

visionPortal = builder.build();

// Disable or re-enable the aprilTag processor at any time.

//visionPortal.setProcessorEnabled(aprilTag, true);

} // end method initAprilTag()

/**
* Add telemetry about AprilTag detections.
*/
private void telemetryAprilTag() {

List<AprilTagDetection> currentDetections = aprilTag.getDetections();

telemetry.addData("# AprilTags Detected", currentDetections.size());

// Step through the list of detections and display info for each one.
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.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)",
detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg,
deg)", detection.ftcPose.range, detection.ftcPose.bearing,
detection.ftcPose.elevation));
} 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));
}
} // end for() loop

// Add "key" information to telemetry

telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
telemetry.addLine("RBE = Range, Bearing & Elevation");

} // end method telemetryAprilTag()

} // end class

/* 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.teamcode;

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")

public class MyConceptBlackboard 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));
}
}

/* Copyright (c) 2017 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.teamcode;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Func;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;

import java.util.Locale;

/*
* This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit
(IMU) from AdaFruit.
*
* Note: this is a Legacy example that will not work with newer Control/Expansion
Hubs that use a different IMU
* Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a
more universal IMU interface.
*
* 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
*
* @see <a href="http://www.adafruit.com/products/2472">Adafruit IMU</a>
*/
@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")

public class MySensorBNO055IMU extends LinearOpMode

{
//-----------------------------------------------------------------------------
-----------------
// State
//-----------------------------------------------------------------------------
-----------------

// The IMU sensor object

BNO055IMU imu;

// State used for updating telemetry

Orientation angles;
Acceleration gravity;

//-----------------------------------------------------------------------------
-----------------
// Main logic
//-----------------------------------------------------------------------------
-----------------

@Override public void runOpMode() {

// Set up the parameters with which we will use our IMU. Note that
integration
// algorithm here just reports accelerations to the logcat log; it doesn't
actually
// provide positional information.
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the
calibration sample OpMode
parameters.loggingEnabled = true;
parameters.loggingTag = "IMU";
parameters.accelerationIntegrationAlgorithm = new
JustLoggingAccelerationIntegrator();

// Retrieve and initialize the IMU. We expect the IMU to be attached to an

I2C port
// on a Core Device Interface Module, configured to be a sensor of type
"AdaFruit IMU",
// and named "imu".
imu = hardwareMap.get(BNO055IMU.class, "imu");
imu.initialize(parameters);

// Set up our telemetry dashboard

composeTelemetry();

// Wait until we're told to go

waitForStart();
 // Start the logging of measured acceleration
imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);

// Loop and update the dashboard

while (opModeIsActive()) {
telemetry.update();
}
}

//-----------------------------------------------------------------------------
-----------------
// Telemetry Configuration
//-----------------------------------------------------------------------------
-----------------

void composeTelemetry() {

// At the beginning of each telemetry update, grab a bunch of data

// from the IMU that we will then display in separate lines.
telemetry.addAction(new Runnable() { @Override public void run()
{
// Acquiring the angles is relatively expensive; we don't want
// to do that in each of the three items that need that info, as
that's
// three times the necessary expense.
angles = imu.getAngularOrientation(AxesReference.INTRINSIC,
AxesOrder.ZYX, AngleUnit.DEGREES);
gravity = imu.getGravity();
}
});

telemetry.addLine()
.addData("status", new Func<String>() {
@Override public String value() {
return imu.getSystemStatus().toShortString();
}
})
.addData("calib", new Func<String>() {
@Override public String value() {
return imu.getCalibrationStatus().toString();
}
});

telemetry.addLine()
.addData("heading", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.firstAngle);
}
})
.addData("roll", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.secondAngle);
}
})
.addData("pitch", new Func<String>() {
@Override public String value() {
return formatAngle(angles.angleUnit, angles.thirdAngle);
}
});
 telemetry.addLine()
.addData("grvty", new Func<String>() {
@Override public String value() {
return gravity.toString();
}
})
.addData("mag", new Func<String>() {
@Override public String value() {
return String.format(Locale.getDefault(), "%.3f",
Math.sqrt(gravity.xAccel*gravity.xAccel
+ gravity.yAccel*gravity.yAccel
+ gravity.zAccel*gravity.zAccel));
}
});
}

//-----------------------------------------------------------------------------
-----------------
// Formatting
//-----------------------------------------------------------------------------
-----------------

String formatAngle(AngleUnit angleUnit, double angle) {

return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
}

String formatDegrees(double degrees){

return String.format(Locale.getDefault(), "%.1f",
AngleUnit.DEGREES.normalize(degrees));
}
}