From 22175373d24e78f13b83b40e5a83991b9aa71641 Mon Sep 17 00:00:00 2001 From: 535tobor <92122791+535tobor@users.noreply.github.com> Date: Thu, 28 Sep 2023 18:10:30 -0400 Subject: [PATCH] Add files via upload --- teamcode/BlueBoardSide.java | 77 +++++++++ teamcode/BlueFarSide.java | 50 ++++++ teamcode/DriveRobot.java | 81 +++++++++ teamcode/RedBoardSide.java | 50 ++++++ teamcode/RedFarSide.java | 51 ++++++ teamcode/TargetOperations.java | 297 +++++++++++++++++++++++++++++++++ 6 files changed, 606 insertions(+) create mode 100644 teamcode/BlueBoardSide.java create mode 100644 teamcode/BlueFarSide.java create mode 100644 teamcode/DriveRobot.java create mode 100644 teamcode/RedBoardSide.java create mode 100644 teamcode/RedFarSide.java create mode 100644 teamcode/TargetOperations.java diff --git a/teamcode/BlueBoardSide.java b/teamcode/BlueBoardSide.java new file mode 100644 index 0000000..3612f9b --- /dev/null +++ b/teamcode/BlueBoardSide.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +@Autonomous(name="Blue, Board Side") +public class BlueBoardSide extends TargetOperations { + + @Override + public void runOpMode() throws InterruptedException { + runInit(); + while(opModeInInit()){runInitLoop();} + waitForStart(); + runStart(); + if (isStopRequested()){runStop();} + while (opModeIsActive()) {runLoop();} + visionPortal.close(); + } + + @Override + public void runInit() { + mapMotors("fr","fl","br","bl"); + forwardMotors(false,true,false,true); + initAprilTag(); + } + + @Override + public void runInitLoop(){} + + @Override + public void runStart() {} + @Override + public void runStop() { + stopAll(); + } + @Override + public void runLoop() { + + // when testing the botman's robot for a test bot make back wheels half speed (fr & fl = 1 then : br & bl = 0.5); + + setAprilTagVariables(); + if (gamepad1.a) { + drive(0.5,0.5, 0.25,0.25); + } + else { + driveZeroPower(); + } + + if (gamepad1.x) { + drive(0.5,-0.5, 0.25,-0.25); + } + else { + driveZeroPower(); + } + + if (gamepad1.b) { + drive(-0.5,0.5, -0.25,0.25); + } + else { + driveZeroPower(); + } + output(tagId); + if (tagId == 1){ + output("The First Tag!"); + runStop(); + } + else if (tagId == 2){ + output("The Second Tag!"); + runStop(); + } + else if (tagId == 3){ + output("The Third Tag!"); + runStop(); + } + + } +} + diff --git a/teamcode/BlueFarSide.java b/teamcode/BlueFarSide.java new file mode 100644 index 0000000..3fa7b8b --- /dev/null +++ b/teamcode/BlueFarSide.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +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; + +@Autonomous(name="Blue, Far Side") +public class BlueFarSide extends TargetOperations { + + @Override + public void runOpMode() { + + runInit(); + while(opModeInInit()){runInitLoop();} + waitForStart(); + runStart(); + if (isStopRequested()){runStop();} + while (opModeIsActive()) {runLoop();} + } + + @Override + public void runInit() { + + } + + @Override + public void runInitLoop() { + + } + + @Override + public void runStart() { + + } + + @Override + public void runLoop() { + + } + + @Override + public void runStop() { + + } +} \ No newline at end of file diff --git a/teamcode/DriveRobot.java b/teamcode/DriveRobot.java new file mode 100644 index 0000000..a46147a --- /dev/null +++ b/teamcode/DriveRobot.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +@TeleOp +public class DriveRobot extends TargetOperations { + + @Override + public void runOpMode() { + runInit(); + while(opModeInInit()){runInitLoop();} + waitForStart(); + runStart(); + if (isStopRequested()){runStop();} + while (opModeIsActive()) {runLoop();} + } + + @Override + public void runInit() { + mapMotors("fr","fl","br","bl"); + forwardMotors(false,true,false,true); + imuGet("imu", "LEFT", "UP"); + } + + @Override + public void runInitLoop() { + + } + + @Override + public void runStart() { + + } + + @Override + public void runLoop() { + double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed + double x = gamepad1.left_stick_x; + double rx = gamepad1.right_stick_x; + + // This button choice was made so that it is hard to hit on accident, + // it can be freely changed based on preference. + // The equivalent button is start on Xbox-style controllers. + if (gamepad1.options) { + imu.resetYaw(); + } + + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + // Rotate the movement direction counter to the bot's rotation + double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); + double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); + + rotX = rotX * 1.1; // Counteract imperfect strafing + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double frontLeftPower = (rotY + rotX + rx) / denominator; + double backLeftPower = (rotY - rotX + rx) / denominator; + double frontRightPower = (rotY - rotX - rx) / denominator; + double backRightPower = (rotY + rotX - rx) / denominator; + + // when testing the botman's robot for a test bot make back wheels half speed (fr & fl = 1 then : br & bl = 0.5); + + fl.setPower(frontLeftPower); + bl.setPower(backLeftPower); + fr.setPower(frontRightPower); + + + // speed is still there, more force given to joystick, more power. + + } + + @Override + public void runStop() { + + } +} \ No newline at end of file diff --git a/teamcode/RedBoardSide.java b/teamcode/RedBoardSide.java new file mode 100644 index 0000000..f530cdd --- /dev/null +++ b/teamcode/RedBoardSide.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +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; + +@Autonomous(name="Red, Board Side") +public class RedBoardSide extends TargetOperations { + @Override + public void runOpMode() { + + runInit(); + while(opModeInInit()){runInitLoop();} + waitForStart(); + runStart(); + if (isStopRequested()){runStop();} + while (opModeIsActive()) {runLoop();} + } + + @Override + public void runInit() { + + } + + @Override + public void runInitLoop() { + + } + + @Override + public void runStart() { + + } + + @Override + public void runLoop() { + + } + + @Override + public void runStop() { + + } + +} \ No newline at end of file diff --git a/teamcode/RedFarSide.java b/teamcode/RedFarSide.java new file mode 100644 index 0000000..f528e57 --- /dev/null +++ b/teamcode/RedFarSide.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +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; + +@Autonomous(name="Red, Far Side") +public class RedFarSide extends TargetOperations { + + @Override + public void runOpMode() { + + runInit(); + while(opModeInInit()){runInitLoop();} + waitForStart(); + runStart(); + if (isStopRequested()){runStop();} + while (opModeIsActive()) {runLoop();} + } + + @Override + public void runInit() { + + } + + @Override + public void runInitLoop() { + + } + + @Override + public void runStart() { + + } + + @Override + public void runLoop() { + + } + + @Override + public void runStop() { + + } + +} \ No newline at end of file diff --git a/teamcode/TargetOperations.java b/teamcode/TargetOperations.java new file mode 100644 index 0000000..49ce778 --- /dev/null +++ b/teamcode/TargetOperations.java @@ -0,0 +1,297 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; + +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.ArrayList; +import java.util.List; +public abstract class TargetOperations extends LinearOpMode { + + // 1 motor for lifting onto arm, 1 for pixels + + public static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + public double firstTagId, tagId; + public AprilTagDetection firstTagFound; + public double tagsFound; + public String tagName; + + List tagCenter, tagPosition; + + /** + * The variable to store our instance of the AprilTag processor. + */ + public AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + public VisionPortal visionPortal; + + DcMotor fr; + DcMotor fl; + DcMotor br; + DcMotor bl; + IMU imu; + + public abstract void runInit(); + public abstract void runInitLoop(); + public abstract void runStart(); + public abstract void runLoop(); + public abstract void runStop(); + + public void mapMotors (String frontLeftMotor, String frontRightMotor, String backLeftMotor, String backRightMotor) { + fr = hardwareMap.dcMotor.get(frontRightMotor); + fl = hardwareMap.dcMotor.get(frontLeftMotor); + br = hardwareMap.dcMotor.get(backRightMotor); + bl = hardwareMap.dcMotor.get(backLeftMotor); + } + + public void forwardMotors (boolean frontLeftMotor, boolean frontRightMotor, boolean backLeftMotor, boolean backRightMotor) { + + if (frontRightMotor) { + fr.setDirection(DcMotorSimple.Direction.FORWARD); + } + + else { + fr.setDirection(DcMotorSimple.Direction.REVERSE); + } + + if (frontLeftMotor) { + fl.setDirection(DcMotorSimple.Direction.FORWARD); + } + + else { + fl.setDirection(DcMotorSimple.Direction.REVERSE); + } + + if (backRightMotor) { + br.setDirection(DcMotorSimple.Direction.FORWARD); + } + + else { + br.setDirection(DcMotorSimple.Direction.REVERSE); + } + + if (backLeftMotor) { + bl.setDirection(DcMotorSimple.Direction.FORWARD); + } + + else { + bl.setDirection(DcMotorSimple.Direction.REVERSE); + } + } + + public void imuGet (String hardwareMapImuName, String logoFacing, String usbFacing) { + // Retrieve the IMU from the hardware map + imu = hardwareMap.get(IMU.class, hardwareMapImuName); + IMU.Parameters parameters = null; + // Adjust the orientation parameters to match your robot + if (logoFacing == "UP" && usbFacing == "FORWARD") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); + } + + else if (logoFacing == "LEFT" && usbFacing == "UP") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.LEFT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + } + + else if (logoFacing == "RIGHT" && usbFacing == "UP") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + } + + else if (logoFacing == "FORWARD" && usbFacing == "UP") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + } + + else if (logoFacing == "BACKWARD" && usbFacing == "UP") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); + } + + else if (logoFacing == "FORWARD" && usbFacing == "RIGHT") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, + RevHubOrientationOnRobot.UsbFacingDirection.RIGHT)); + } + + else if (logoFacing == "UP" && usbFacing == "BACKWARD") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.UP, + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); + } + + else if (logoFacing == "DOWN" && usbFacing == "FORWARD") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.DOWN, + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); + } + + else if (logoFacing == "FORWARD" && usbFacing == "LEFT") { + parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.FORWARD, + RevHubOrientationOnRobot.UsbFacingDirection.LEFT)); + } + imu.initialize(parameters); + } + + // hardcoded movements: + public void forward(double speed) { + fr.setPower(speed); + fl.setPower(speed); + br.setPower(speed); + bl.setPower(speed); + + } + + public void backward(double speed) { + fr.setPower(speed); + fl.setPower(speed); + br.setPower(speed); + bl.setPower(speed); + + } + + public void left(double speed) { + fr.setPower(speed); + fl.setPower(-speed); + br.setPower(-speed); + bl.setPower(speed); + + } + + public void right(double speed) { + fr.setPower(-speed); + fl.setPower(speed); + br.setPower(speed); + bl.setPower(-speed); + + } + + public void forwardLeft(double speed) { + fr.setPower(speed); + bl.setPower(speed); + + } + + public void forwardRight(double speed) { + fl.setPower(speed); + br.setPower(speed); + + } + + public void backwardLeft(double speed) {; + fl.setPower(-speed); + br.setPower(-speed); + + } + + public void backwardRight(double speed) { + fr.setPower(-speed); + bl.setPower(-speed); + + } + + public void turnRight(double speed) { + fr.setPower(-speed); + fl.setPower(speed); + br.setPower(-speed); + bl.setPower(speed); + + } + + public void turnLeft(double speed) { + fr.setPower(speed); + fl.setPower(-speed); + br.setPower(speed); + bl.setPower(-speed); + + } + + public void drive(double frontLeftSpeed, double frontRightSpeed, double backLeftSpeed, double backRightSpeed) { + fr.setPower(frontLeftSpeed); + fl.setPower(frontRightSpeed); + br.setPower(backLeftSpeed); + bl.setPower(backRightSpeed); + + } + + public void driveZeroPower() { + drive(0,0,0,0); + } + + public void output(Object output) { + telemetry.addData("", output); + telemetry.update(); + } + + public void initAprilTag(String CameraName) { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, CameraName), aprilTag); + } + + public void initAprilTag() { + // default hardware map name for the camera + initAprilTag("Camera 1"); + } + + public void setAprilTagVariables() { + + List currentDetections = aprilTag.getDetections(); + firstTagFound = currentDetections.get(0); + firstTagId = firstTagFound.id; + tagsFound = currentDetections.size(); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + + tagName = detection.metadata.name; + tagPosition = new ArrayList(); + tagPosition.add(detection.ftcPose.x); + tagPosition.add(detection.ftcPose.y); + tagPosition.add(detection.ftcPose.z); + tagPosition.add(detection.ftcPose.pitch); + tagPosition.add(detection.ftcPose.roll); + tagPosition.add(detection.ftcPose.yaw); + tagPosition.add(detection.ftcPose.range); + tagPosition.add(detection.ftcPose.bearing); + tagPosition.add(detection.ftcPose.elevation); + + } + else { + + tagName = "unknown"; + } + + tagId = detection.id; + + tagCenter = new ArrayList(); + tagCenter.add(detection.center.x); + tagCenter.add(detection.center.y); + } + } + + public void stopAll() { + requestOpModeStop(); + } +}