Robot course- IMU implemented

https://stemrobotics.cs.pdx.edu/node/4196.html

 https://stemrobotics.cs.pdx.edu/node/7266.html

https://stemrobotics.cs.pdx.edu/node/7266%3Froot=4196.html

https://docs.revrobotics.com/duo-control/sensors/i2c/imu

For 2022-2023 powerplay

https://github.com/technototes/PowerPlay2022/blob/main/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java

Or 

https://github.com/technototes/PowerPlay2022/blob/main/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java

https://github.com/technototes/PowerPlay2022/blob/main/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java

Arm control - game pad

 https://docs.revrobotics.com/duo-control/programming/hello-robot-autonomous-robot/arm-control-onbot-java

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

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
u/TeleOp(name="Basic: MyFirstLinear OpMode", group="Linear Opmode")
//@Disabled
public class BasicOmniOpMode_Linear extends LinearOpMode { 

// Declare OpMode members.
static DcMotor leftTank;
static DcMotor rightTank;
static DcMotor elbow;
static DcMotor arm;
static CRServo clawr;
static CRServo clawl;
static Servo pivotClaw;
ElapsedTime ET = new ElapsedTime();
boolean button_bumper_right_already_pressed2 = false;
boolean button_bumper_left_already_pressed2 = false;
boolean button_a_already_pressed1 = false;
boolean button_b_already_pressed1 = false;
boolean button_y_already_pressed1 = false;
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).
leftTank = hardwareMap.get(DcMotor.class, "leftTank");
rightTank = hardwareMap.get(DcMotor.class, "rightTank");
elbow = hardwareMap.get(DcMotor.class, "elbow");
arm = hardwareMap.get(DcMotor.class, "arm");
clawr = hardwareMap.get(CRServo.class, "clawr");
clawl = hardwareMap.get(CRServo.class, "clawl");
pivotClaw = hardwareMap.get(Servo.class, "pivotClaw");
leftTank.setDirection(DcMotor.Direction.REVERSE);
rightTank.setDirection(DcMotor.Direction.FORWARD);
arm.setDirection(DcMotor.Direction.FORWARD);
elbow.setDirection(DcMotor.Direction.FORWARD);
clawr.setDirection(CRServo.Direction.FORWARD);
clawl.setDirection(CRServo.Direction.REVERSE);
pivotClaw.setDirection(Servo.Direction.FORWARD);
//clawr.scaleRange(0, 0.35);
//clawl.scaleRange(0, 0.35);
clawl.setPower(-1);
clawr.setPower(-1);
sleep(1000);
clawr.setPower(0);
clawl.setPower(0);
leftTank.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightTank.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
elbow.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
leftTank.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightTank.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// Wait for the game to start (driver presses PLAY)
waitForStart();
ET.reset();
while (opModeIsActive()) { 

double lTPower = gamepad1.left_stick_y;
double rTPower = gamepad1.right_stick_y;
//double armPower = -gamepad2.left_stick_y;
//double elbowPower = -gamepad2.right_stick_y;
leftTank.setPower(lTPower);
rightTank.setPower(rTPower);
arm.setPower(0.5);
elbow.setPower(0.5);
if (gamepad2.right_bumper) {
clawr.setPower(1);
clawl.setPower(1);
} 

if (gamepad2.left_bumper) {
clawr.setPower(1);
clawl.setPower(-1);
} 

//set elbow
if (!button_bumper_left_already_pressed2) {
if (gamepad2.dpad_down) {
elbow.setTargetPosition(100000);
elbow.setMode(DcMotor.RunMode.RUN_TO_POSITION);
elbow.setPower(0.8);
arm.setTargetPosition(1000);
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
arm.setPower(0.5);
button_bumper_left_already_pressed2 = true;
}
} else {
button_bumper_left_already_pressed2 = false;
} 

if (!button_bumper_right_already_pressed2) {
if (gamepad2.y) {
elbow.setTargetPosition(0);
elbow.setMode(DcMotor.RunMode.RUN_TO_POSITION);
elbow.setPower(1);
arm.setTargetPosition(0);
arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
arm.setPower(1);
button_bumper_right_already_pressed2 = true;
}
} else {
button_bumper_right_already_pressed2 = false;
} 

if (!button_a_already_pressed1) {
if (gamepad1.a) {
pivotClaw.setPosition(0);
button_a_already_pressed1 = true;
}
} else {
(button_a_already_pressed1) = false;
} 

if (!button_b_already_pressed1) {
if (gamepad1.b) {
pivotClaw.setPosition(.25);
button_b_already_pressed1 = true;
}
} else {
button_b_already_pressed1 = false;
} 

if (!button_y_already_pressed1) {
if (gamepad1.y) {
pivotClaw.setPosition(.5);
button_y_already_pressed1 = true;
}
} else {
button_y_already_pressed1 = false;
}
}
}
}

FTC PowerPlay Tutorial for Camera Vision (AprilTags)

 android-studio-guide.pdf (firstinspires.org)

https://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/android-studio-guide.pdf

https://youtu.be/GXeSsbGXURM

https://github.com/ftctechnh/ftc_app/wiki/Java-Sample-TensorFlow-Object-Detection-Op-Mode

Team code

 https://github.com/FTC-ORBIT/14029-Powerplay/pull/58/files

Camera code

 

/*
	* Copyright (c) 2021 OpenFTC Team
	*
	* Permission is hereby granted, free of charge, to any person obtaining a copy
	* of this software and associated documentation files (the "Software"), to deal
	* in the Software without restriction, including without limitation the rights
	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
	* copies of the Software, and to permit persons to whom the Software is
	* furnished to do so, subject to the following conditions:
	*
	* The above copyright notice and this permission notice shall be included in all
	* copies or substantial portions of the Software.
	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
	* SOFTWARE.
	*/
	package org.firstinspires.ftc.teamcode.auton;
	import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
	import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
	import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
	import org.openftc.apriltag.AprilTagDetection;
	import org.openftc.easyopencv.OpenCvCamera;
	import org.openftc.easyopencv.OpenCvCameraFactory;
	import org.openftc.easyopencv.OpenCvCameraRotation;
	import org.openftc.easyopencv.OpenCvInternalCamera;
	import java.util.ArrayList;
	@TeleOp
	public class AprilTagAutonomousInitDetectionExample extends LinearOpMode
	{
	OpenCvCamera camera;
	AprilTagDetectionPipeline aprilTagDetectionPipeline;
	static final double FEET_PER_METER = 3.28084;
	// Lens intrinsics
	// UNITS ARE PIXELS
	// NOTE: this calibration is for the C920 webcam at 800x448.
	// You will need to do your own calibration for other configurations!
	double fx = 578.272;
	double fy = 578.272;
	double cx = 402.145;
	double cy = 221.506;
	// UNITS ARE METERS
	double tagsize = 0.166;
	// Tag ID 1,2,3 from the 36h11 family
	int LEFT = 1;
	int MIDDLE = 2;
	int RIGHT = 3;
	AprilTagDetection tagOfInterest = null;
	@Override
	public void runOpMode()
	{
	int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
	camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId);
	aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagsize, fx, fy, cx, cy);
	camera.setPipeline(aprilTagDetectionPipeline);
	camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener()
	{
	@Override
	public void onOpened()
	{
	camera.startStreaming(800,448, OpenCvCameraRotation.UPRIGHT);
	}
	@Override
	public void onError(int errorCode)
	{
	}
	});
	telemetry.setMsTransmissionInterval(50);
	/*
	* The INIT-loop:
	* This REPLACES waitForStart!
	*/
	while (!isStarted() && !isStopRequested())
	{
	ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
	if(currentDetections.size() != 0)
	{
	boolean tagFound = false;
	for(AprilTagDetection tag : currentDetections)
	{
	if(tag.id == LEFT || tag.id == MIDDLE || tag.id == RIGHT)
	{
	tagOfInterest = tag;
	tagFound = true;
	break;
	}
	}
	if(tagFound)
	{
	telemetry.addLine("Tag of interest is in sight!\n\nLocation data:");
	tagToTelemetry(tagOfInterest);
	}
	else
	{
	telemetry.addLine("Don't see tag of interest :(");
	if(tagOfInterest == null)
	{
	telemetry.addLine("(The tag has never been seen)");
	}
	else
	{
	telemetry.addLine("\nBut we HAVE seen the tag before; last seen at:");
	tagToTelemetry(tagOfInterest);
	}
	}
	}
	else
	{
	telemetry.addLine("Don't see tag of interest :(");
	if(tagOfInterest == null)
	{
	telemetry.addLine("(The tag has never been seen)");
	}
	else
	{
	telemetry.addLine("\nBut we HAVE seen the tag before; last seen at:");
	tagToTelemetry(tagOfInterest);
	}
	}
	telemetry.update();
	sleep(20);
	}
	/*
	* The START command just came in: now work off the latest snapshot acquired
	* during the init loop.
	*/
	/* Update the telemetry */
	if(tagOfInterest != null)
	{
	telemetry.addLine("Tag snapshot:\n");
	tagToTelemetry(tagOfInterest);
	telemetry.update();
	}
	else
	{
	telemetry.addLine("No tag snapshot available, it was never sighted during the init loop :(");
	telemetry.update();
	}
	/* Actually do something useful */
	if(tagOfInterest == null){
	//default trajectory here if preferred
	}else if(tagOfInterest.id == LEFT){
	//left trajectory
	}else if(tagOfInterest.id == MIDDLE){
	//middle trajectory
	}else{
	//right trajectory
	}
	/* You wouldn't have this in your autonomous, this is just to prevent the sample from ending */
	while (opModeIsActive()) {sleep(20);}
	}
	void tagToTelemetry(AprilTagDetection detection)
	{
	telemetry.addLine(String.format("\nDetected tag ID=%d", detection.id));
	telemetry.addLine(String.format("Translation X: %.2f feet", detection.pose.x*FEET_PER_METER));
	telemetry.addLine(String.format("Translation Y: %.2f feet", detection.pose.y*FEET_PER_METER));
	telemetry.addLine(String.format("Translation Z: %.2f feet", detection.pose.z*FEET_PER_METER));
	telemetry.addLine(String.format("Rotation Yaw: %.2f degrees", Math.toDegrees(detection.pose.yaw)));
	telemetry.addLine(String.format("Rotation Pitch: %.2f degrees", Math.toDegrees(detection.pose.pitch)));
	telemetry.addLine(String.format("Rotation Roll: %.2f degrees", Math.toDegrees(detection.pose.roll)));
	}
	}