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 c980378..a8d1fb9 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 @@ -22,6 +22,7 @@ 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; @@ -77,12 +78,14 @@ public class Red extends LinearOpMode { shooter.setShooterMode("MANUAL"); - shooter.sethoodPosition(0.54); + shooter.sethoodPosition(0.53); transfer = new Transfer(robot); transfer.setTransferPosition(transferServo_out); + Intake intake = new Intake(robot); + @@ -94,11 +97,36 @@ public class Red extends LinearOpMode { .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-10), h1 ); + while(opModeInInit()) { + shooter.setTurretPosition(0.33); + aprilTag.initTelemetry(); aprilTag.update(); + shooter.update(); TELE.update(); } @@ -123,7 +151,6 @@ public class Red extends LinearOpMode { ) ); - shooter.moveTurret(angle); transfer.setTransferPower(1); @@ -133,83 +160,251 @@ public class Red extends LinearOpMode { double stamp = getRuntime(); - boolean gpp =false; - boolean pgp = false; - boolean ppg = false; + stamp = getRuntime(); - aprilTag.turnTelemetryOn(true); - - while(getRuntime()-stamp <4.5) { - - aprilTag.update(); + while (getRuntime()-stamp<4.5) { - gpp = aprilTag.isDetected(21); - pgp = aprilTag.isDetected(22); + shooter.moveTurret(0.31); - ppg = aprilTag.isDetected(23); + double time = getRuntime()-stamp; - 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) { + } 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 < 2.5) { - - - - transfer.transferIn(); - } else if (time < 4) { - transfer.transferOut(); - - spindexer.outtake1(); - - - - ticker++; - } else if (time < 4.5) { + } else if (time < 3.0) { transfer.transferIn(); - } else if (time < 6) { + } else if (time < 4.0) { transfer.transferOut(); spindexer.outtake1(); - } else if (time < 6.5) { + } 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 index 9ded639..d3f1208 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.constants; import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; @Config public class Poses { @@ -14,6 +15,15 @@ public class Poses { 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 = 75, y2_b = 25, h2_b = Math.toRadians(135); + + + public static double x3 = 45, y3 = 53, h3 = Math.toRadians(135); + + public static Pose2d teleStart = new Pose2d(x1,-10,0); + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index 6341a84..45ad2e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -73,9 +73,9 @@ public final class MecanumDrive { public double kA = 0.000046; // path profile parameters (in inches) - public double maxWheelVel = 80; + public double maxWheelVel = 120; public double minProfileAccel = -30; - public double maxProfileAccel = 80; + public double maxProfileAccel = 120; // turn profile parameters (in radians) public double maxAngVel = 2* Math.PI; // shared with path 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 6cea691..2f43a59 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -118,7 +120,7 @@ public class TeleopV1 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + drive = new MecanumDrive(hardwareMap, teleStart); @@ -216,7 +218,7 @@ public class TeleopV1 extends LinearOpMode { if (isStopRequested()) return; - drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0)); + drive = new MecanumDrive(hardwareMap, teleStart); while(opModeIsActive()){ 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 new file mode 100644 index 0000000..d2f3bc5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java @@ -0,0 +1,59 @@ +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(); + + } + + + } +}