diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java deleted file mode 100644 index 84087b5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue.java +++ /dev/null @@ -1,478 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Poses.h1; -import static org.firstinspires.ftc.teamcode.constants.Poses.h2; -import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b; -import static org.firstinspires.ftc.teamcode.constants.Poses.h3; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.Poses.x1; -import static org.firstinspires.ftc.teamcode.constants.Poses.x2; -import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b; -import static org.firstinspires.ftc.teamcode.constants.Poses.x3; -import static org.firstinspires.ftc.teamcode.constants.Poses.y1; -import static org.firstinspires.ftc.teamcode.constants.Poses.y2; -import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b; -import static org.firstinspires.ftc.teamcode.constants.Poses.y3; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.subsystems.AprilTag; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; -import org.firstinspires.ftc.teamcode.subsystems.Spindexer; -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.utils.Robot; - - -@Config -@Autonomous (preselectTeleOp = "TeleopV1") -public class Blue extends LinearOpMode { - - Robot robot; - - MultipleTelemetry TELE; - - MecanumDrive drive; - - AprilTag aprilTag; - - Shooter shooter; - - - - public static double angle = 0.1; - - Spindexer spindexer; - - Transfer transfer; - - public class shooterOn implements Action { - - int ticker = 1; - - double stamp = 0.0; - - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - if (ticker ==1){ - stamp = getRuntime(); - - } - - ticker ++; - - if (getRuntime() - stamp < 0.2){ - return true; - } else if (getRuntime() - stamp < 0.4) { - - shooter.setManualPower(1); - - shooter.update(); - - return true; - } else { - - return false; - - } - } - } - - - - - - @Override - public void runOpMode() throws InterruptedException { - - - - - robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0,0,0 - )); - - aprilTag = new AprilTag(robot, TELE); - - shooter = new Shooter(robot, TELE); - - spindexer = new Spindexer(robot, TELE); - - spindexer.outtake3(); - - shooter.setShooterMode("MANUAL"); - - shooter.sethoodPosition(0.53); - - transfer = new Transfer(robot); - - transfer.setTransferPosition(transferServo_out); - - Intake intake = new Intake(robot); - - robot.hood.setPosition(hoodDefault); - - - - - - - - - TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - - TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .turnTo(Math.toRadians(-135)) - .strafeToLinearHeading(new Vector2d(x2, -y2), -h2 ); - - - TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, -y2, -h2)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - - .strafeToLinearHeading(new Vector2d(x2_b, -y2_b), -h2_b ) - - .strafeToLinearHeading(new Vector2d(x3, -y3), -h3 ); - - - TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, -y3, -h3)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - - TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x1, y1-30), h1 ); - - while(opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()){ - hoodDefault -= 0.02; - } - - if (gamepad2.dpadDownWasPressed()){ - hoodDefault += 0.02; - } - - robot.hood.setPosition(hoodDefault); - - shooter.setTurretPosition(0.3); - - aprilTag.initTelemetry(); - - aprilTag.update(); - shooter.update(); - - TELE.addData("hood", hoodDefault); - - TELE.update(); - - - } - - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()){ - - robot.hood.setPosition(hoodDefault); - - transfer.setTransferPower(1); - - transfer.update(); - - - - - Actions.runBlocking( - new ParallelAction( - traj1.build(), - new shooterOn() - ) - ); - - - transfer.setTransferPower(1); - - transfer.update(); - - shooter.setManualPower(1); - - double stamp = getRuntime(); - - - stamp = getRuntime(); - - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.3); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(1); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - - robot.intake.setPower(1); - - Actions.runBlocking( - traj2.build() - ); - - - Actions.runBlocking( - traj3.build() - ); - - shooter.setManualPower(1); - - - - - robot.intake.setPower(0); - - spindexer.outtake3(); - - - stamp = getRuntime(); - - shooter.setManualPower(1); - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.3); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(1); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - robot.intake.setPower(1); - - Actions.runBlocking( - traj4.build() - ); - - - Actions.runBlocking( - traj5.build() - ); - - shooter.setManualPower(1); - - - - - robot.intake.setPower(0); - - - stamp = getRuntime(); - - shooter.setManualPower(1); - - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.3); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(1); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - - Actions.runBlocking( - traj6.build() - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - - TELE.update(); - - sleep (2000); - - - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java deleted file mode 100644 index 5da26ae..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java +++ /dev/null @@ -1,431 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Poses.*; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.constants.ServoPositions; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.subsystems.AprilTag; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; -import org.firstinspires.ftc.teamcode.subsystems.Spindexer; -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.utils.Robot; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - - - -@Config -@Autonomous -public class Red extends LinearOpMode { - - Robot robot; - - MultipleTelemetry TELE; - - MecanumDrive drive; - - AprilTag aprilTag; - - Shooter shooter; - - - - public static double angle = 0.1; - - Spindexer spindexer; - - Transfer transfer; - - - - - - @Override - public void runOpMode() throws InterruptedException { - - - - - robot = new Robot(hardwareMap); - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0,0,0 - )); - - aprilTag = new AprilTag(robot, TELE); - - shooter = new Shooter(robot, TELE); - - spindexer = new Spindexer(robot, TELE); - - spindexer.outtake3(); - - shooter.setShooterMode("MANUAL"); - - shooter.sethoodPosition(hoodDefault); - - transfer = new Transfer(robot); - - transfer.setTransferPosition(transferServo_out); - - Intake intake = new Intake(robot); - - robot.hood.setPosition(hoodDefault); - - - - - - - - - TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - - TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .turnTo(Math.toRadians(135)) - .strafeToLinearHeading(new Vector2d(x2, y2), h2 ); - - - TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - - .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b ) - - .strafeToLinearHeading(new Vector2d(x3, y3), h3 ); - - - TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); - - - TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x1, y1+30), h1 ); - - while(opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()){ - hoodDefault -= 0.02; - } - - if (gamepad2.dpadDownWasPressed()){ - hoodDefault += 0.02; - } - - robot.hood.setPosition(hoodDefault); - - shooter.setTurretPosition(0.33); - - aprilTag.initTelemetry(); - - aprilTag.update(); - shooter.update(); - - TELE.update(); - } - - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()){ - - robot.hood.setPosition(hoodDefault); - - transfer.setTransferPower(1); - - transfer.update(); - - - - - Actions.runBlocking( - new ParallelAction( - traj1.build() - ) - ); - - - transfer.setTransferPower(1); - - transfer.update(); - - shooter.setManualPower(1); - - double stamp = getRuntime(); - - - stamp = getRuntime(); - - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.31); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(0); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - - robot.intake.setPower(1); - - Actions.runBlocking( - traj2.build() - ); - - - Actions.runBlocking( - traj3.build() - ); - - shooter.setManualPower(1); - - - - - robot.intake.setPower(0); - - spindexer.outtake3(); - - - stamp = getRuntime(); - - shooter.setManualPower(1); - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.31); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(0); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - robot.intake.setPower(1); - - Actions.runBlocking( - traj4.build() - ); - - - Actions.runBlocking( - traj5.build() - ); - - shooter.setManualPower(1); - - - - - robot.intake.setPower(0); - - - stamp = getRuntime(); - - shooter.setManualPower(1); - - while (getRuntime()-stamp<4.5) { - - - - shooter.moveTurret(0.31); - - double time = getRuntime()-stamp; - - - if (time < 0.3) { - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 1) { - - spindexer.outtake3(); - - - } else if (time < 1.4) { - - - transfer.transferIn(); - } else if (time < 2.6) { - transfer.transferOut(); - - spindexer.outtake2(); - - - - } else if (time < 3.0) { - - - transfer.transferIn(); - } else if (time < 4.0) { - - transfer.transferOut(); - - spindexer.outtake1(); - - } else if (time < 4.4) { - transfer.transferIn(); - } else { - - transfer.transferOut(); - spindexer.outtake3(); - - shooter.setManualPower(0); - - } - - transfer.update(); - - shooter.update(); - - spindexer.update(); - - } - - spindexer.outtake3(); - - Actions.runBlocking( - traj6.build() - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addLine("finished"); - - TELE.update(); - - sleep (2000); - - - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java deleted file mode 100644 index 6cc3514..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; - -@Config -public class Poses { - - public static double goalHeight = 42; //in inches - - public static double turretHeight = 12; - - public static double relativeGoalHeight = goalHeight - turretHeight; - - public static double x1 = 50, y1 = 0, h1 = 0; - - public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135); - - public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135); - - - public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135); - - public static Pose2d teleStart = new Pose2d(x1,-10,0); - - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java deleted file mode 100644 index beb7514..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - - -import com.acmerobotics.dashboard.config.Config; - -@Config -public class ServoPositions { - - public static double spindexer_intakePos = 0.665; - - public static double spindexer_outtakeBall3 = 0.845; - - public static double spindexer_outtakeBall2 = 0.48; - public static double spindexer_outtakeBall1 = 0.1; - - public static double transferServo_out = 0.13; - - public static double transferServo_in = 0.4; - - public static double hoodDefault = 0.35; - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java deleted file mode 100644 index ed0d7b3..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTag.java +++ /dev/null @@ -1,238 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; - -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Subsystem; -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.Collections; -import java.util.List; - -public class AprilTag implements Subsystem { - - private AprilTagProcessor aprilTag; - private VisionPortal visionPortal; - - private MultipleTelemetry TELE; - - private boolean teleOn = false; - - private int detections = 0; - - List currentDetections; - - ArrayList> Data = new ArrayList<>(); - - - - public AprilTag(Robot robot, MultipleTelemetry tele) { - - - - this.aprilTag = robot.aprilTagProcessor; - - - this.TELE = tele; - - - - - } - - @Override - public void update() { - - currentDetections = aprilTag.getDetections(); - - UpdateData(); - - if(teleOn){ - tagTELE(); - initTelemetry(); - } - - - - - } - - public void initTelemetry (){ - - TELE.addData("Camera Preview", "Check Driver Station for stream"); - TELE.addData("Status", "Initialized - Press START"); - - - - } - - public void tagTELE () { - - TELE.addData("# AprilTags Detected", detections); - - // Display info for each detected tag - for (ArrayList detection : Data) { - if (detection.get(0) ==1) { - // Known AprilTag with metadata - TELE.addLine(String.format("\n==== (ID %d) %s ====", - detection.get(1).intValue(), "")); - - TELE.addLine(String.format("XYZ: %6.1f %6.1f %6.1f (inch)", - detection.get(2), - detection.get(3), - detection.get(4))); - - TELE.addData("Distance", getDistance(detection.get(1).intValue())); - - TELE.addLine(String.format("PRY: %6.1f %6.1f %6.1f (deg)", - detection.get(5), - detection.get(6), - detection.get(7))); - - TELE.addLine(String.format("RBE: %6.1f %6.1f %6.1f (inch, deg, deg)", - detection.get(8), - detection.get(9), - detection.get(10))); - } - } - } - - - public void turnTelemetryOn(boolean bool) { - - teleOn = bool; - - } - - - - public void UpdateData () { - - Data.clear(); // <--- THIS FIXES YOUR ISSUE - - - detections = currentDetections.size(); - - - for (AprilTagDetection detection : currentDetections) { - - ArrayList detectionData = new ArrayList(); - - - - if (detection.metadata != null) { - - detectionData.add(1.0); - // Known AprilTag with metadata - - detectionData.add( (double) detection.id); - - - - detectionData.add(detection.ftcPose.x); - detectionData.add(detection.ftcPose.y); - detectionData.add(detection.ftcPose.z); - - - - detectionData.add(detection.ftcPose.pitch); - detectionData.add(detection.ftcPose.roll); - detectionData.add(detection.ftcPose.yaw); - - detectionData.add(detection.ftcPose.range); - detectionData.add(detection.ftcPose.bearing); - detectionData.add(detection.ftcPose.elevation); - - } else { - - detectionData.add(0, 0.0); - - } - - Data.add(detectionData); - } - - } - - public int getDetectionCount() { - - return detections; - - } - - public boolean isDetected (int id){ - return (!filterID(Data, (double) id ).isEmpty()); - } - - - - public double getDistance(int id) { - ArrayList d = filterID(Data, (double) id); - if (d.size() >= 5) { - double x = d.get(2); - double y = d.get(3); - double z = d.get(4); - return Math.sqrt(x*x + y*y + z*z); - } - return -1; // tag not found - } - - // Returns the position as [x, y, z] - public List getPosition(int id) { - ArrayList d = filterID(Data, (double) id); - if (d.size() >= 5) { - List pos = new ArrayList<>(); - pos.add(d.get(2)); - pos.add(d.get(3)); - pos.add(d.get(4)); - return pos; - } - return Collections.emptyList(); - } - - // Returns orientation as [pitch, roll, yaw] - public List getOrientation(int id) { - ArrayList d = filterID(Data, (double) id); - if (d.size() >= 8) { - List ori = new ArrayList<>(); - ori.add(d.get(5)); - ori.add(d.get(6)); - ori.add(d.get(7)); - return ori; - } - return Collections.emptyList(); - } - - // Returns range, bearing, elevation as [range, bearing, elevation] - public List getRBE(int id) { - ArrayList d = filterID(Data, (double) id); - if (d.size() >= 11) { - List rbe = new ArrayList<>(); - rbe.add(d.get(8)); - rbe.add(d.get(9)); - rbe.add(d.get(10)); - return rbe; - } - return Collections.emptyList(); - } - - // Returns full raw data for debugging or custom processing - public ArrayList getRawData(int id) { - return filterID(Data, (double) id); - } - - public static ArrayList filterID(ArrayList> data, double x) { - for (ArrayList innerList : data) { - // Ensure it has a second element - if (innerList.size() > 1 && Math.abs(innerList.get(1) - x) < 1e-9) { - return innerList; // Return the first match - } - } - // Return an empty ArrayList if no match found - return new ArrayList<>(); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java index e192049..0e41422 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -4,31 +4,25 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.gamepad.GamepadEx; import com.arcrobotics.ftclib.gamepad.GamepadKeys; import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Subsystem; import java.util.Objects; -public class Drivetrain implements Subsystem { - - private GamepadEx gamepad; +public class Drivetrain { + private final GamepadEx gamepad; + private final DcMotorEx fl; + private final DcMotorEx fr; + private final DcMotorEx bl; + private final DcMotorEx br; public MultipleTelemetry TELE; - private String Mode = "Default"; - - private DcMotorEx fl; - - private DcMotorEx fr; - private DcMotorEx bl; - private DcMotorEx br; - private double defaultSpeed = 0.7; private double slowSpeed = 0.3; - public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){ + + public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) { this.fl = robot.frontLeft; this.fr = robot.frontRight; @@ -39,23 +33,21 @@ public class Drivetrain implements Subsystem { this.TELE = tele; - - } - public void setMode (String mode){ + public void setMode(String mode) { this.Mode = mode; } - public void setDefaultSpeed (double speed){ + public void setDefaultSpeed(double speed) { this.defaultSpeed = speed; } - public void setSlowSpeed (double speed){ + public void setSlowSpeed(double speed) { this.slowSpeed = speed; } - public void RobotCentric(double fwd, double strafe, double turn, double turbo){ + public void RobotCentric(double fwd, double strafe, double turn, double turbo) { double y = -fwd; // Remember, Y stick value is reversed double x = strafe * 1.1; // Counteract imperfect strafing @@ -70,15 +62,13 @@ public class Drivetrain implements Subsystem { double frontRightPower = (y - x - rx) / denominator; double backRightPower = (y + x - rx) / denominator; - fl.setPower(frontLeftPower*turbo); - bl.setPower(backLeftPower*turbo); - fr.setPower(frontRightPower*turbo); - br.setPower(backRightPower*turbo); + fl.setPower(frontLeftPower * turbo); + bl.setPower(backLeftPower * turbo); + fr.setPower(frontRightPower * turbo); + br.setPower(backRightPower * turbo); } - - @Override public void update() { if (Objects.equals(Mode, "Default")) { @@ -88,7 +78,7 @@ public class Drivetrain implements Subsystem { gamepad.getRightX(), gamepad.getLeftX(), (gamepad.getTrigger( - GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed) + GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed) - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed + defaultSpeed ) @@ -96,4 +86,4 @@ public class Drivetrain implements Subsystem { } } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java deleted file mode 100644 index c55fb2a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Gamepad; - - -import org.firstinspires.ftc.teamcode.utils.Robot; - -public class Intake implements Subsystem { - - private GamepadEx gamepad; - - public MultipleTelemetry TELE; - - - private DcMotorEx intake; - - private double intakePower = 1.0; - - private int intakeState = 0; - - - public Intake (Robot robot){ - - - this.intake = robot.intake; - - - } - - public int getIntakeState() { - return intakeState; - } - - public void toggle(){ - if (intakeState !=0){ - intakeState = 0; - } else { - intakeState = 1; - } - } - - public void setIntakePower(double pow){ - intakePower = pow; - } - - public void intake(){ - intakeState =1; - } - - public void reverse(){ - intakeState =-1; - } - - - public void stop(){ - intakeState =-1; - } - - - - - @Override - public void update() { - - if (intakeState == 1){ - intake.setPower(intakePower); - } else if (intakeState == -1){ - intake.setPower(-intakePower); - } else { - intake.setPower(0); - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java deleted file mode 100644 index 7bb38ee..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ /dev/null @@ -1,337 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; -import com.arcrobotics.ftclib.controller.PIDController; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.PIDCoefficients; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; -import org.firstinspires.ftc.teamcode.constants.Poses; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.subsystems.Subsystem; - -import java.util.Objects; - -public class Shooter implements Subsystem { - private final DcMotorEx fly1; - private final DcMotorEx fly2; - - private final DcMotorEx encoder; - private final Servo hoodServo; - - private final Servo turret1; - - private final Servo turret2; - - - private final MultipleTelemetry telemetry; - - private boolean telemetryOn = false; - - private double manualPower = 0.0; - private double hoodPos = 0.0; - - private double turretPos = 0.0; - private double velocity = 0.0; - private double posPower = 0.0; - - private int targetPosition = 0; - - private double p = 0.0003, i = 0, d = 0.00001; - - private PIDController controller; - - - - - - private String shooterMode = "AUTO"; - - private String turretMode = "AUTO"; - - - public Shooter(Robot robot, MultipleTelemetry TELE) { - this.fly1 = robot.shooter1; - this.fly2 = robot.shooter2; - this.telemetry = TELE; - this.hoodServo = robot.hood; - - // Reset encoders - fly1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - fly2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - - controller = new PIDController(p, i, d); - - - controller.setPID(p, i, d); - - this.turret1 = robot.turr1; - - this.turret2 = robot.turr2; - - this.encoder = robot.shooterEncoder; - - - - - - - } - - public void telemetryUpdate() { - - // Telemetry - - telemetry.addData("Mode", shooterMode); - telemetry.addData("ManualPower", manualPower); - telemetry.addData("Position", getPosition()); - telemetry.addData("TargetPosition", targetPosition); - telemetry.addData("Velocity", getVelocity()); - telemetry.addData("TargetVelocity", velocity); - telemetry.addData("hoodPos", gethoodPosition()); - telemetry.addData("turretPos", getTurretPosition()); - telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d); - telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS)); - telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS)); - - } - - public double gethoodPosition() { - return (hoodServo.getPosition()); - } - - public void sethoodPosition(double pos) {hoodPos = pos;} - - public double getTurretPosition() { - return ((turret1.getPosition()+ (1-turret2.getPosition()))/2); - } - - public void setTurretPosition(double pos) {turretPos = pos;} - - public double getVelocity() { - return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2)); - } - - public void setVelocity(double vel){velocity = vel;} - - - - public void setPosPower(double power){posPower = power;} - - - public void setTargetPosition(int pos){ - targetPosition = pos; - } - - public void setTolerance(int tolerance){ - controller.setTolerance(tolerance); - } - - public void setControllerCoefficients(double kp, double ki, double kd){ - p = kp; - i = ki; - d = kd; - controller.setPID(p, i, d); - - } - - public PIDCoefficients getControllerCoefficients(){ - - return new PIDCoefficients(p, i, d); - - } - - - - - - - - public void setManualPower(double power){manualPower = power;} - - public String getShooterMode(){return shooterMode;} - - public String getTurretMode(){return turretMode;} - - - public double getPosition(){ - return ((double) ((fly1.getCurrentPosition() + fly2.getCurrentPosition()) /2)); - } - - public void setShooterMode(String mode){ shooterMode = mode;} - - public void setTurretMode(String mode){ turretMode = mode;} - - - public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){ - - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - - - Pose2d deltaPose = new Pose2d( - goalPose.position.x - robotPose.position.x, - goalPose.position.y - robotPose.position.y, - goalPose.heading.toDouble() - (robotPose.heading.toDouble()) - ); - - double distance = Math.sqrt( - deltaPose.position.x * deltaPose.position.x - + deltaPose.position.y * deltaPose.position.y - + Poses.relativeGoalHeight * Poses.relativeGoalHeight - ); - - telemetry.addData("dst", distance); - - double shooterPow = getPowerByDist(distance); - - double hoodAngle = getAngleByDist(distance); - - -// hoodServo.setPosition(hoodAngle); - - moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); - - return distance; - - - - - - //0.9974 * 355 - - - } - - public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ - - double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); - - - double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x)); - - telemetry.addData("deltaAngle", deltaAngle); - - - - if (deltaAngle > 90) { - deltaAngle -=360; - } - - - -// deltaAngle += aTanAngle; - - deltaAngle /= (335); - - telemetry.addData("dAngle", deltaAngle); - - telemetry.addData("AtanAngle", aTanAngle); - - - return ((0.30-deltaAngle) + offset); - - - - } - - //62, 0.44 - - //56.5, 0.5 - - - public double getPowerByDist(double dist){ - - //TODO: ADD LOGIC - return dist; - } - - public double getAngleByDist(double dist){ - - - double newDist = dist - 56.5; - - double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46; - - - - - return pos; - } - - - - - public void setTelemetryOn(boolean state){telemetryOn = state;} - - public void moveTurret(double pos){ - turret1.setPosition(pos); - turret2.setPosition(1-pos); - } - - @Override - public void update() { - - if (Objects.equals(shooterMode, "MANUAL")){ - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - - fly1.setPower(manualPower); - fly2.setPower(manualPower); - } - - else if (Objects.equals(shooterMode, "VEL")){ - - fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - fly1.setVelocity(velocity); - - fly2.setPower(fly1.getPower()); - - - - - - - - } - - else if (Objects.equals(shooterMode, "POS")){ - - - double powPID = controller.calculate(getPosition(), targetPosition); - - - - fly1.setPower(powPID); - fly2.setPower(powPID); - } - - if (Objects.equals(turretMode, "MANUAL")){ -// hoodServo.setPosition(hoodPos); - - moveTurret(turretPos); - - - } - - - - if (telemetryOn) {telemetryUpdate();} - - - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java deleted file mode 100644 index e91820a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java +++ /dev/null @@ -1,257 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.DigitalChannel; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -import java.util.ArrayList; - -public class Spindexer implements Subsystem{ - - private Servo s1; - private Servo s2; - - private DigitalChannel p0; - - private DigitalChannel p1; - private DigitalChannel p2; - private DigitalChannel p3; - private DigitalChannel p4; - - private DigitalChannel p5; - - private AnalogInput input; - - private AnalogInput input2; - - - private MultipleTelemetry TELE; - - private double position = 0.501; - - private boolean telemetryOn = false; - - private boolean ball0 = false; - - private boolean ball1 = false; - - private boolean ball2 = false; - - private boolean green0 = false; - - private boolean green1 = false; - - private boolean green2 = false; - - - - - public Spindexer (Robot robot, MultipleTelemetry tele){ - - this.s1 = robot.spin1; - this.s2 = robot.spin2; - - this.p0 = robot.pin0; - this.p1 = robot.pin1; - this.p2 = robot.pin2; - this.p3 = robot.pin3; - this.p4 = robot.pin4; - this.p5 = robot.pin5; - - this.input = robot.analogInput; - - this.input2 = robot.analogInput2; - - this.TELE = tele; - - } - - public void setTelemetryOn(boolean state){ - telemetryOn = state; - } - - public void colorSensorTelemetry() { - - - TELE.addData("ball0", ball0); - TELE.addData("ball1", ball1); - TELE.addData("ball2", ball2); - TELE.addData("green0", green0); - TELE.addData("green1", green1); - TELE.addData("green2", green2); - - - - } - - public void checkForBalls() { - - if (p0.getState()){ - ball0 = true; - green0 = p1.getState(); - } else { - ball0 = false; - } - - if (p2.getState()){ - ball1 = true; - green1 = p3.getState(); - } else { - ball1 = false; - } - - if (p4.getState()){ - ball2 = true; - green2 = p5.getState(); - } else { - ball2 = false; - } - } - - public void setPosition (double pos) { - position = pos; - } - - public void intake () { - position = spindexer_intakePos; - } - - public void intakeShake(double runtime) { - if ((runtime % 0.25) >0.125) { - position = spindexer_intakePos + 0.04; - } else { - position = spindexer_intakePos - 0.04; - - } - } - - public void outtake3Shake(double runtime) { - if ((runtime % 0.25) >0.125) { - position = spindexer_outtakeBall3 + 0.04; - } else { - position = spindexer_outtakeBall3 - 0.04; - - } - } - - - public void outtake3 () { - position = spindexer_outtakeBall3; - } - - public void outtake2 () { - position = spindexer_outtakeBall2; - } - - public void outtake1 () { - position = spindexer_outtakeBall1; - } - - - public int outtakeGreen(int secLast, int Last) { - if (green2 && (secLast!=3) && (Last!=3)) { - outtake3(); - return 3; - } else if (green1 && (secLast!=2) && (Last!=2)){ - outtake2(); - return 2; - } else if (green0 && (secLast!=1) && (Last!=1)) { - outtake1(); - return 1; - } else { - - if (secLast!=1 && Last!= 1){ - outtake1(); - return 1; - } else if (secLast!=2 && Last!=2){ - outtake2(); - return 2; - } else { - outtake3(); - return 3; - } - - } - } - - - - public void outtakeGreenFs() { - if (green0 && ball0) { - outtake1(); - } else if (green1 && ball1){ - outtake2(); - } else if (green2 && ball2) { - outtake3(); - } - } - - public int greens() { - int num = 0; - - if (green0){num++;} - - if (green1){num++;} - - - if (green2){num++;} - - return num; - - - } - - - public int outtakePurple(int secLast, int Last) { - if (!green2 && (secLast!=3) && (Last!=3)) { - outtake3(); - return 3; - } else if (!green1 && (secLast!=2) && (Last!=2)){ - outtake2(); - return 2; - } else if (!green0 && (secLast!=1) && (Last!=1)) { - outtake1(); - return 1; - } else { - - if (secLast!=1 && Last!= 1){ - outtake1(); - return 1; - } else if (secLast!=2 && Last!=2){ - outtake2(); - return 2; - } else { - outtake3(); - return 3; - } - - - } - } - - - - - @Override - public void update() { - - if (position !=0.501) { - - s1.setPosition(position); - s2.setPosition(1 - position); - } - - - if (telemetryOn) { - colorSensorTelemetry(); - } - - - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java deleted file mode 100644 index 34bf534..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java +++ /dev/null @@ -1,6 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -public interface Subsystem { - - public void update(); -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java deleted file mode 100644 index 0b6f5f9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Transfer.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; - -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -public class Transfer implements Subsystem{ - - private final Servo servo; - - private final DcMotorEx transfer; - - private double motorPow = 0.0; - - private double servoPos = 0.501; - - public Transfer (Robot robot){ - - this.servo = robot.transferServo; - - this.transfer = robot.transfer; - - } - - public void setTransferPosition(double pos){ - this.servoPos = pos; - } - - public void setTransferPower (double pow){ - this.motorPow = pow; - } - - public void transferOut(){ - this.setTransferPosition(transferServo_out); - } - - public void transferIn(){ - this.setTransferPosition(transferServo_in); - } - - - - - - @Override - public void update() { - - if (servoPos!=0.501){ - servo.setPosition(servoPos); - } - - transfer.setPower(motorPow); - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java deleted file mode 100644 index 0bd4026..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/HoldTest.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import static org.firstinspires.ftc.teamcode.constants.Poses.h1; -import static org.firstinspires.ftc.teamcode.constants.Poses.x1; -import static org.firstinspires.ftc.teamcode.constants.Poses.y1; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; -import com.acmerobotics.roadrunner.ftc.PinpointIMU; -import com.pedropathing.ftc.localization.localizers.PinpointLocalizer; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Robot; - - -@TeleOp -@Config -public class HoldTest extends LinearOpMode { - - Robot robot; - MecanumDrive drive; - - public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); - public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); - - - @Override - public void runOpMode() throws InterruptedException { - - MultipleTelemetry TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - robot = new Robot(hardwareMap); - - drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); - - - TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(0.01, 0.01), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT); - - waitForStart(); - - - while (opModeIsActive()){ - - drive.updatePoseEstimate(); - - Pose2d currentPos = drive.localizer.getPose(); - - - - TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) - .strafeToLinearHeading(new Vector2d(0, 0), 0, VEL_CONSTRAINT, ACCEL_CONSTRAINT); - - if (Math.abs(currentPos.position.x) >0.5 || Math.abs(currentPos.position.y)>0.5) { - Actions.runBlocking( - traj2.build() - ); - } - - - - - - } - - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java deleted file mode 100644 index 27f3af5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ /dev/null @@ -1,901 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.Pose2d; -import com.arcrobotics.ftclib.gamepad.GamepadEx; -import com.arcrobotics.ftclib.gamepad.GamepadKeys; -import com.arcrobotics.ftclib.gamepad.ToggleButtonReader; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; -import org.firstinspires.ftc.teamcode.subsystems.Intake; -import org.firstinspires.ftc.teamcode.subsystems.Shooter; -import org.firstinspires.ftc.teamcode.subsystems.Spindexer; -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.utils.Robot; - - -@Config -@TeleOp - -public class TeleopV1 extends LinearOpMode { - - - Robot robot; - - Drivetrain drivetrain; - - Intake intake; - - Spindexer spindexer; - - Transfer transfer; - - MultipleTelemetry TELE; - - GamepadEx g1; - - GamepadEx g2; - - public static double defaultSpeed = 1; - - public static double slowMoSpeed = 0.4; - - public static double power = 0.0; - - public static double pos = hoodDefault; - - public boolean all = false; - - public int ticker = 0; - - ToggleButtonReader g1RightBumper; - - ToggleButtonReader g2Circle; - - ToggleButtonReader g2Square; - - - ToggleButtonReader g2Triangle; - - ToggleButtonReader g2RightBumper; - - ToggleButtonReader g1LeftBumper; - - ToggleButtonReader g2LeftBumper; - - ToggleButtonReader g2DpadUp; - - ToggleButtonReader g2DpadDown; - - ToggleButtonReader g2DpadRight; - - ToggleButtonReader g2DpadLeft; - - public boolean leftBumper = false; - public double g1RightBumperStamp = 0.0; - - public double g1LeftBumperStamp = 0.0; - - - public double g2LeftBumperStamp = 0.0; - - public static int spindexerPos = 0; - - public boolean green = false; - - Shooter shooter; - - public boolean scoreAll = false; - - MecanumDrive drive ; - - public boolean autotrack = false; - - public int last = 0; - public int second = 0; - - public double offset = 0.0; - - public static double rIn = 0.59; - - public static double rOut = 0; - - public boolean notShooting = true; - - public boolean circle = false; - - public boolean square = false; - - public boolean tri = false; - - - - @Override - public void runOpMode() throws InterruptedException { - - drive = new MecanumDrive(hardwareMap, teleStart); - - - - - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - FtcDashboard.getInstance().getTelemetry(), - telemetry - ); - - g1 = new GamepadEx(gamepad1); - - g1RightBumper = new ToggleButtonReader( - g1, GamepadKeys.Button.RIGHT_BUMPER - ); - - g2 = new GamepadEx(gamepad2); - - g1LeftBumper = new ToggleButtonReader( - g1, GamepadKeys.Button.LEFT_BUMPER - ); - - g2Circle = new ToggleButtonReader( - g2, GamepadKeys.Button.B - ); - - g2Triangle = new ToggleButtonReader( - g2, GamepadKeys.Button.Y - ); - - g2Square = new ToggleButtonReader( - g2, GamepadKeys.Button.X - ); - - g2RightBumper = new ToggleButtonReader( - g2, GamepadKeys.Button.RIGHT_BUMPER - ); - - - g2LeftBumper = new ToggleButtonReader( - g2, GamepadKeys.Button.LEFT_BUMPER - ); - - g2DpadUp = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_UP - ); - - - g2DpadDown = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_DOWN - ); - - g2DpadLeft = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_LEFT - ); - - - g2DpadRight = new ToggleButtonReader( - g2, GamepadKeys.Button.DPAD_RIGHT - ); - - - - - drivetrain = new Drivetrain(robot, TELE, g1); - - drivetrain.setMode("Default"); - - drivetrain.setDefaultSpeed(defaultSpeed); - - drivetrain.setSlowSpeed(slowMoSpeed); - - intake = new Intake(robot); - - transfer = new Transfer(robot); - - - spindexer = new Spindexer(robot, TELE); - - spindexer.setTelemetryOn(true); - - shooter = new Shooter(robot, TELE); - - shooter.setShooterMode("MANUAL"); - - robot.rejecter.setPosition(rIn); - - - - - - waitForStart(); - - if (isStopRequested()) return; - - drive = new MecanumDrive(hardwareMap, teleStart); - - - while(opModeIsActive()){ - - drive.updatePoseEstimate(); - - TELE.addData("pose", drive.localizer.getPose()); - - TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - - - TELE.addData("off", offset); - - - robot.hood.setPosition(pos); - - g1LeftBumper.readValue(); - - if (g1LeftBumper.wasJustPressed()){ - g2LeftBumperStamp = getRuntime(); - - - - spindexer.intakeShake(getRuntime()); - - leftBumper = true; - } - - if (leftBumper){ - double time = getRuntime() - g2LeftBumperStamp; - - if (time < 1.0){ - robot.rejecter.setPosition(rOut); - } else { - robot.rejecter.setPosition(rIn); - } - - } - - - - - intake(); - - drivetrain.update(); - - TELE.update(); - - transfer.update(); - - g2RightBumper.readValue(); - - g2LeftBumper.readValue(); - - g2DpadDown.readValue(); - - g2DpadUp.readValue(); - - if (!scoreAll){ - spindexer.checkForBalls(); - } - - if(g2DpadUp.wasJustPressed()){ - pos -=0.02; - } - - if(g2DpadDown.wasJustPressed()){ - pos +=0.02; - } - - g2DpadLeft.readValue(); - - g2DpadRight.readValue(); - - if(g2DpadLeft.wasJustPressed()){ - offset -=0.02; - } - - if(g2DpadRight.wasJustPressed()){ - offset +=0.02; - } - - - - TELE.addData("hood", pos); - - - - - - - - - if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { - - - - - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); - - } else { - - autotrack = false; - - shooter.moveTurret(0.3+offset); - - } - - if (gamepad2.right_stick_button){ - autotrack = true; - } - - - - if (g2RightBumper.wasJustPressed()){ - transfer.setTransferPower(1); - transfer.transferIn(); - shooter.setManualPower(1); - - notShooting = false; - - } - - if (g2RightBumper.wasJustReleased()){ - transfer.setTransferPower(1); - transfer.transferOut(); - } - - if (gamepad2.left_stick_y>0.5){ - - shooter.setManualPower(0); - } else if (gamepad2.left_stick_y<-0.5){ - shooter.setManualPower(1); - } - - if (g2LeftBumper.wasJustPressed()){ - g2LeftBumperStamp = getRuntime(); - notShooting = false; - scoreAll = true; - } - - if (scoreAll) { - double time = getRuntime() - g2LeftBumperStamp; - - - shooter.setManualPower(1); - - TELE.addData("greenImportant", green); - - TELE.addData("last", last); - TELE.addData("2ndLast", second); - - int numGreen = spindexer.greens(); - - if (square) { - - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - second = last; - } else { - last = spindexer.outtakeGreen(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - - transfer.transferIn(); - } else if (time < 6) { - - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else if (tri) { - - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - } else { - last = spindexer.outtakeGreen(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - - transfer.transferIn(); - } else if (time < 6) { - - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else if (circle){ - - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - - transfer.transferIn(); - } else if (time < 6) { - - - transfer.transferOut(); - - if (ticker == 0) { - - if (numGreen == 2) { - last = spindexer.outtakePurple(second, last); - } else { - last = spindexer.outtakeGreen(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - } else{ - - - if (time < 0.3) { - - ticker = 0; - - last = 0; - second = 0; - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - } else { - all = true; - } - - - transfer.transferOut(); - transfer.setTransferPower(1); - } else if (time < 2) { - - if (ticker == 0) { - - if (all) { - spindexer.outtake3(); - last = 3; - second = 3; - } else if (green) { - last = spindexer.outtakeGreen(second, last); - second = last; - } else { - last = spindexer.outtakePurple(second, last); - second = last; - - } - } - - second = last; - - ticker++; - - - } else if (time < 2.5) { - - ticker = 0; - - second = last; - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - - - } - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - if (ticker == 0) { - - if (all) { - spindexer.outtake2(); - - last = 2; - } else if (green) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - } else if (time < 4.5) { - - ticker = 0; - - - if (gamepad2.right_trigger > 0.5) { - green = false; - - all = gamepad2.left_trigger > 0.5; - - } else if (gamepad2.left_trigger > 0.5) { - green = true; - - all = false; - } - - transfer.transferIn(); - } else if (time < 6) { - - - transfer.transferOut(); - - if (ticker == 0) { - - if (all) { - spindexer.outtake1(); - } else if (green) { - last = spindexer.outtakeGreen(second, last); - } else { - last = spindexer.outtakePurple(second, last); - - } - } - - ticker++; - - } else if (time < 6.5) { - transfer.transferIn(); - } else { - - ticker = 0; - - - scoreAll = false; - transfer.transferOut(); - - shooter.setManualPower(0); - - } - - - } - } - - - shooter.update(); - - - - - - - - - - - - - } - - - - - } - - public void intake(){ - - - g1RightBumper.readValue(); - - g2Circle.readValue(); - - g2Square.readValue(); - - g2Triangle.readValue(); - - if (g1RightBumper.wasJustPressed()){ - - notShooting = true; - - - - - if (getRuntime() - g1RightBumperStamp < 0.3){ - intake.reverse(); - } else { - intake.toggle(); - } - - if (intake.getIntakeState()==1){ - shooter.setManualPower(0); - } - - - - - spindexer.intake(); - - transfer.transferOut(); - - - g1RightBumperStamp = getRuntime(); - - } - - - if (intake.getIntakeState()==1 && notShooting) { - - spindexer.intakeShake(getRuntime()); - - } else { - if (g2Circle.wasJustPressed()){ - circle = true; - tri = false; - square = false; - - - - } - - if (g2Triangle.wasJustPressed()){ - circle = false; - tri = true; - square = false; - } - - if (g2Square.wasJustPressed()){ - circle = false; - tri = false; - square = true; - } - - if (gamepad2.x){ - circle = false; - tri = false; - square = false; - } - - - - - } - - - intake.update(); - - - - - spindexer.update(); - - - - - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java deleted file mode 100644 index 30ae795..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TransferTest.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - - -import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.subsystems.Transfer; -import org.firstinspires.ftc.teamcode.utils.Robot; - -@Config -@TeleOp -public class TransferTest extends LinearOpMode { - - Robot robot; - - - Transfer transfer; - - public static double pos = 0.40; - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - transfer = new Transfer(robot); - - waitForStart(); - - while (opModeIsActive()){ - transfer.setTransferPosition(pos); - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java deleted file mode 100644 index d2f3bc5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.subsystems.AprilTag; - - -@TeleOp -@Config -public class WebcamTest extends LinearOpMode { - - AprilTag webcam; - - MultipleTelemetry TELE; - - Robot robot; - - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - - webcam = new AprilTag(robot, TELE); - - webcam.turnTelemetryOn(true); - - - - - while(opModeInInit()){ - - webcam.initTelemetry(); - - TELE.update(); - - }; - - if(isStopRequested()) return; - - - while (opModeIsActive()){ - - webcam.update(); - - TELE.update(); - - } - - - } -}