Robotics: December 2022

Thursday, December 29, 2022

For 2022-2023 powerplay

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

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

Tuesday, December 27, 2022

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;
}
}
}
}

Saturday, December 24, 2022

Team code

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

Thursday, December 22, 2022

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)));
	}
	}

Robotics

Thursday, December 29, 2022

Robot course- IMU implemented

Tuesday, December 27, 2022

Arm control - game pad

Monday, December 26, 2022

FTC PowerPlay Tutorial for Camera Vision (AprilTags)

Saturday, December 24, 2022

Team code

Thursday, December 22, 2022

Camera code

We are the Winner at the regional tournament !

Report Abuse