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 index c05b5f8..c980378 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java @@ -1,11 +1,30 @@ 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.Shooter; +import org.firstinspires.ftc.teamcode.subsystems.Spindexer; +import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -19,17 +38,180 @@ public class Red extends LinearOpMode { 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(0.54); + + transfer = new Transfer(robot); + + transfer.setTransferPosition(transferServo_out); + + + + + + + + + TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1 ); + + + while(opModeInInit()) { + + aprilTag.initTelemetry(); + + aprilTag.update(); + + TELE.update(); + } + + waitForStart(); if (isStopRequested()) return; if (opModeIsActive()){ + transfer.setTransferPower(1); + + transfer.update(); + + + + + Actions.runBlocking( + new ParallelAction( + traj1.build() + ) + ); + + shooter.moveTurret(angle); + + transfer.setTransferPower(1); + + transfer.update(); + + shooter.setManualPower(1); + + double stamp = getRuntime(); + + boolean gpp =false; + + boolean pgp = false; + boolean ppg = false; + + aprilTag.turnTelemetryOn(true); + + while(getRuntime()-stamp <4.5) { + + aprilTag.update(); + + + gpp = aprilTag.isDetected(21); + + pgp = aprilTag.isDetected(22); + + ppg = aprilTag.isDetected(23); + + TELE.update(); + } + + robot.turr1.setPosition(0.9); + + robot.turr2.setPosition(0.1); + + int ticker = 0; + + if (gpp){ + if (time < 0.3) { + + ticker = 0; + + + transfer.transferOut(); + transfer.setTransferPower(1); + } else if (time < 2) { + + spindexer.outtake2(); + + + + } else if (time < 2.5) { + + + + transfer.transferIn(); + } else if (time < 4) { + transfer.transferOut(); + + spindexer.outtake1(); + + + + ticker++; + } else if (time < 4.5) { + + + transfer.transferIn(); + } else if (time < 6) { + + transfer.transferOut(); + + spindexer.outtake1(); + + } else if (time < 6.5) { + transfer.transferIn(); + } else { + + transfer.transferOut(); + + shooter.setManualPower(0); + + } + + } + + + + } } 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 index 7fdb907..9ded639 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -12,5 +12,8 @@ public class Poses { public static double relativeGoalHeight = goalHeight - turretHeight; + public static double x1 = 50, y1 = 0, h1 = 0; + + } 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 index d648c40..6cea691 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -293,7 +293,7 @@ public class TeleopV1 extends LinearOpMode { offset +=0.02; } - if (gamepad2.right_stick_x > 0.5){ + if (gamepad2.right_stick_y < 0.7){ pos = shooter.getAngleByDist( shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset) ); @@ -320,7 +320,7 @@ public class TeleopV1 extends LinearOpMode { autotrack = false; - shooter.moveTurret(shooter.getTurretPosition() - gamepad2.right_stick_x* 0.02); + shooter.moveTurret(shooter.getTurretPosition() + gamepad2.right_stick_x* 0.02); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 3411afa..19a779e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -9,7 +9,11 @@ import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.openftc.easyopencv.OpenCvWebcam; public class Robot { @@ -57,6 +61,13 @@ public class Robot { public AnalogInput analogInput2; + public AprilTagProcessor aprilTagProcessor; + + + public WebcamName webcam; + + + @@ -128,6 +139,12 @@ public class Robot { transfer.setDirection(DcMotorSimple.Direction.REVERSE); + aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); + + + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + + } }