From e8bf2033ad087216d2601fdea08aa44ab7e322f3 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 17 Jan 2026 00:53:05 -0600 Subject: [PATCH] yayyy --- .../ftc/teamcode/autonomous/AutoClose_V3.java | 34 +- .../ftc/teamcode/autonomous/AutoFar_V1.java | 34 +- .../ftc/teamcode/autonomous/Blue_V2.java | 839 ------------------ .../autonomous/ProtoAutoClose_V3.java | 754 ++++++++++++++++ .../ftc/teamcode/autonomous/Red_V2.java | 771 ---------------- .../ftc/teamcode/autonomous/blank.java | 4 - .../ftc/teamcode/constants/Poses.java | 10 + .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/libs/RR/MecanumDrive.java | 385 ++++---- .../ftc/teamcode/teleop/TeleopV2.java | 4 - .../ftc/teamcode/teleop/TeleopV3.java | 96 +- .../ftc/teamcode/tests/PIDServoTest.java | 12 +- .../ftc/teamcode/tests/ShooterTest.java | 4 - .../utils/PositionalServoProgrammer.java | 13 +- .../ftc/teamcode/utils/Robot.java | 8 +- 15 files changed, 1047 insertions(+), 1923 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java index b6ab1c0..1bcee9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java @@ -103,17 +103,17 @@ public class AutoClose_V3 extends LinearOpMode { if (gpp || pgp || ppg) { if (redAlliance){ robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redClose); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); + double turretPID = turret_redClose; + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(-turretPID); + return false; } else { robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueClose); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueClose); + double turretPID = turret_blueClose; + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(-turretPID); + return false; } } else { return true; @@ -188,8 +188,8 @@ public class AutoClose_V3 extends LinearOpMode { return true; } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { robot.transferServo.setPosition(transferServo_out); - robot.turr1.setPower(holdTurrPow); - robot.turr2.setPower(holdTurrPow); + robot.turr1.setPosition(holdTurrPow); + robot.turr2.setPosition(holdTurrPow); TELE.addData("Velocity", velo); TELE.addLine("shot once"); TELE.update(); @@ -411,7 +411,7 @@ public class AutoClose_V3 extends LinearOpMode { double turrPID; if (redAlliance){ - turrPID = servo.setTurrPos(turret_detectRedClose); + turrPID = turret_detectRedClose; shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); @@ -430,7 +430,7 @@ public class AutoClose_V3 extends LinearOpMode { shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); } else { - turrPID = servo.setTurrPos(turret_detectBlueClose); + turrPID = turret_detectBlueClose; shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); @@ -450,8 +450,8 @@ public class AutoClose_V3 extends LinearOpMode { .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); } - robot.turr1.setPower(turrPID); - robot.turr2.setPower(-turrPID); + robot.turr1.setPosition(turrPID); + robot.turr2.setPosition(-turrPID); robot.hood.setPosition(hoodAuto); @@ -566,9 +566,9 @@ public class AutoClose_V3 extends LinearOpMode { } } double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); + double turretPID = turretPos; + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(-turretPID); } public void shootingSequence() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java index 88dd86e..1dc53eb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java @@ -103,18 +103,18 @@ public class AutoFar_V1 extends LinearOpMode { if (gpp || pgp || ppg) { if (redAlliance){ robot.limelight.pipelineSwitch(3); - double turretPID = servo.setTurrPos(turret_redFar); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redFar); + robot.turr1.setPosition(turret_redFar); + robot.turr2.setPosition(-turret_redFar); } else { robot.limelight.pipelineSwitch(2); - double turretPID = servo.setTurrPos(turret_blueFar); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_blueFar); + + robot.turr1.setPosition(turret_blueFar); + robot.turr2.setPosition(-turret_blueFar); + } + + return false; } else { return true; } @@ -188,8 +188,8 @@ public class AutoFar_V1 extends LinearOpMode { return true; } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut && transferIn) { robot.transferServo.setPosition(transferServo_out); - robot.turr1.setPower(holdTurrPow); - robot.turr2.setPower(holdTurrPow); + robot.turr1.setPosition(holdTurrPow); + robot.turr2.setPosition(holdTurrPow); TELE.addData("Velocity", velo); TELE.addLine("shot once"); TELE.update(); @@ -399,13 +399,13 @@ public class AutoFar_V1 extends LinearOpMode { double turrPID; if (redAlliance){ - turrPID = servo.setTurrPos(turret_detectRedClose); + turrPID = turret_detectRedClose; } else { - turrPID = servo.setTurrPos(turret_detectBlueClose); + turrPID = turret_detectBlueClose; } - robot.turr1.setPower(turrPID); - robot.turr2.setPower(-turrPID); + robot.turr1.setPosition(turrPID); + robot.turr2.setPosition(-turrPID); robot.hood.setPosition(hoodAutoFar); @@ -467,9 +467,9 @@ public class AutoFar_V1 extends LinearOpMode { } } double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); + double turretPID = turretPos; + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(-turretPID); } public void shootingSequence() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java deleted file mode 100644 index 792b6db..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java +++ /dev/null @@ -1,839 +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 static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; -import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; -import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*; - -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.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.Robot; - -@Config -@Autonomous(preselectTeleOp = "TeleopV2") -public class Blue_V2 extends LinearOpMode { - - Robot robot; - - MultipleTelemetry TELE; - - MecanumDrive drive; - - AprilTagWebcam aprilTag; - - Flywheel flywheel; - - double velo = 0.0; - double targetVelocity = 0.0; - public static double intake1Time = 2.9; - - public static double intake2Time = 2.9; - - public static double colorDetect = 3.0; - - boolean gpp = false; - - boolean pgp = false; - - boolean ppg = false; - - double powPID = 0.0; - - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - - boolean spindexPosEqual(double spindexer) { - TELE.addData("Velocity", velo); - TELE.addLine("spindex equal"); - TELE.update(); - return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 && - scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); - } - - public Action initShooter(int vel) { - return new Action() { - double initPos = 0.0; - double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double stamp2 = 0.0; - double currentPos = 0.0; - boolean steady = false; - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp2 = getRuntime(); - } - - targetVelocity = (double) vel; - ticker++; - if (ticker % 16 == 0) { - stamp = getRuntime(); - stamp1 = stamp; - } - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.transfer.setPower(1); - - TELE.addData("Velocity", velo); - TELE.update(); - if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){ - steady = true; - stamp2 = getRuntime(); - return true; - } else if (steady && getRuntime() - stamp2 > 1.5){ - TELE.addData("Velocity", velo); - TELE.addLine("finished init"); - TELE.update(); - return false; - } else { - return true; - } - } - }; - } - - public Action steadyShooter(int vel, boolean last) { - return new Action() { - double stamp = 0.0; - boolean steady = false; - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - steady = flywheel.getSteady(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.transfer.setPower(1); - - TELE.addData("Velocity", velo); - TELE.update(); - - - if (last && !steady){ - stamp = getRuntime(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - return false; - } else if (steady) { - stamp = getRuntime(); - return true; - } else { - return true; - } - } - }; - } - - public Action Obelisk() { - return new Action() { - double stamp = getRuntime(); - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if (aprilTag.getTagById(21) != null) { - gpp = true; - } else if (aprilTag.getTagById(22) != null) { - pgp = true; - } else if (aprilTag.getTagById(23) != null) { - ppg = true; - } - aprilTag.update(); - - TELE.addData("Velocity", velo); - TELE.addData("21", gpp); - TELE.addData("22", pgp); - TELE.addData("23", ppg); - TELE.update(); - - if (gpp || pgp || ppg){ - robot.turr1.setPower(turret_blueClose); - robot.turr2.setPower(1 - turret_blueClose); - return false; - } else { - return true; - } - } - }; - } - - public Action intakeReject() { - return new Action() { - double stamp = 0.0; - int ticker = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - double position = 0.0; - - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.02; - } else { - position = spindexer_intakePos1 - 0.02; - } - - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if (getRuntime() - stamp < 0.3){ - return true; - }else { - robot.intake.setPower(0); - return false; - } - } - }; - } - - public Action spindex (double spindexer, double vel){ - return new Action() { - double currentPos = 0.0; - double stamp = 0.0; - double initPos = 0.0; - double stamp1 = 0.0; - int ticker = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - ticker++; - if (ticker % 8 == 0) { - currentPos = (double) robot.shooter1.getCurrentPosition() / 2048; - stamp = getRuntime(); - velo = -60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; - stamp1 = stamp; - } - - if (vel - velo > 500 && ticker > 16) { - powPID = 1.0; - } else if (velo - vel > 500 && ticker > 16){ - powPID = 0.0; - } else if (Math.abs(vel - velo) < 100 && ticker > 16){ - double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - } - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.spin1.setPower(spindexer); - robot.spin2.setPower(1-spindexer); - TELE.addData("Velocity", velo); - TELE.addLine("spindex"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - return !spindexPosEqual(spindexer); - } - }; - } - - public Action Shoot(double vel) { - return new Action() { - double transferStamp = 0.0; - int ticker = 1; - boolean transferIn = false; - double currentPos = 0.0; - double stamp = 0.0; - double initPos = 0.0; - double stamp1 = 0.0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - if (ticker % 8 == 0) { - currentPos = (double) robot.shooter1.getCurrentPosition() / 2048; - stamp = getRuntime(); - velo = -60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; - stamp1 = stamp; - } - - if (vel - velo > 500 && ticker > 16) { - powPID = 1.0; - } else if (velo - vel > 500 && ticker > 16){ - powPID = 0.0; - } else if (Math.abs(vel - velo) < 100 && ticker > 16){ - double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; - - // --- PROPORTIONAL CORRECTION --- - double error = vel - velo; - double correction = kP * error; - - // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); - - // --- FINAL MOTOR POWER --- - powPID = feed + correction; - - // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - } - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - - if (ticker == 1) { - transferStamp = getRuntime(); - ticker++; - } - if (getRuntime() - transferStamp > waitTransfer && !transferIn) { - robot.transferServo.setPosition(transferServo_in); - TELE.addData("Velocity", velo); - TELE.addData("ticker", ticker); - TELE.update(); - transferIn = true; - return true; - } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){ - robot.transferServo.setPosition(transferServo_out); - TELE.addData("Velocity", velo); - TELE.addLine("shot once"); - TELE.update(); - return false; - } else { - return true; - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double position = 0.0; - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.02; - } else { - position = spindexer_intakePos1 - 0.02; - } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - - TELE.addData("Velocity", velo); - TELE.addLine("Intaking"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(1); - if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { - return false; - } else { - return true; - } - } - }; - } - - public Action ColorDetect() { - return new Action() { - double stamp = 0.0; - int ticker = 0; - double position = 0.0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.02; - } else { - position = spindexer_intakePos1 - 0.02; - } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (s1D < 40) { - - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - - - if (gP >= 0.4) { - b1 = 2; - } else { - b1 = 1; - } - } - - if (s2D < 40) { - - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b2 = 2; - } else { - b2 = 1; - } - } - - if (s3D < 30) { - - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b3 = 2; - } else { - b3 = 1; - } - } - - TELE.addData("Velocity", velo); - TELE.addLine("Detecting"); - TELE.addData("Distance 1", s1D); - TELE.addData("Distance 2", s2D); - TELE.addData("Distance 3", s3D); - TELE.addData("B1", b1); - TELE.addData("B2", b2); - TELE.addData("B3", b3); - TELE.update(); - - return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - flywheel = new Flywheel(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - aprilTag = new AprilTagWebcam(); - - TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) - .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); - - TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) - - .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) - - .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); - - TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); - - aprilTag.init(robot, TELE); - - while (opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()) { - hoodAuto-= 0.01; - } - - if (gamepad2.dpadDownWasPressed()) { - hoodAuto += 0.01; - } - - robot.hood.setPosition(hoodAuto); - - robot.turr1.setPower(turret_detectBlueClose); - robot.turr2.setPower(1 - turret_detectBlueClose); - - robot.transferServo.setPosition(transferServo_out); - - robot.spin1.setPower(spindexer_intakePos1); - robot.spin2.setPower(1 - spindexer_intakePos1); - - aprilTag.update(); - TELE.addData("Velocity", velo); - TELE.update(); - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.hood.setPosition(hoodAuto); - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - initShooter(AUTO_CLOSE_VEL), - Obelisk() - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - - robot.hood.setPosition(hoodAuto); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - intake(intake1Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot1.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - intake(intake2Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot2.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() - ) - ); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", velo); - TELE.addLine("finished"); - TELE.update(); - - sleep(2000); - - } - - } - - public void shootingSequence() { - TELE.addData("Velocity", velo); - if (gpp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (pgp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence4(); - TELE.addLine("sequence4"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2) { - sequence4(); - TELE.addLine("sequence4"); - } - } else { - sequence3(); - TELE.addLine("sequence3"); - } - } else if (ppg) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } - } else { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - TELE.update(); - } - - public void sequence1() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence2() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence3() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence4() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence5() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence6() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java new file mode 100644 index 0000000..f1e44e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java @@ -0,0 +1,754 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Poses.*; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.by2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry2c; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turret_blueClose; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turret_redClose; +import static org.firstinspires.ftc.teamcode.constants.ShooterVars.AUTO_CLOSE_VEL; + +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.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.FlywheelV2; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; + +import java.util.List; + +@Config +@Autonomous(preselectTeleOp = "TeleopV3") +public class ProtoAutoClose_V3 extends LinearOpMode { + public static double intake1Time = 2.7; + public static double intake2Time = 3.0; + public static double colorDetect = 3.0; + public static double holdTurrPow = 0.01; // power to hold turret in place + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + FlywheelV2 flywheel; + Servos servo; + double velo = 0.0; + boolean gpp = false; + boolean pgp = false; + boolean ppg = false; + double powPID = 0.0; + double bearing = 0.0; + int b1 = 0; // 0 = no ball, 1 = green, 2 = purple + int b2 = 0;// 0 = no ball, 1 = green, 2 = purple + int b3 = 0;// 0 = no ball, 1 = green, 2 = purple + + public Action initShooter(int vel) { + return new Action() { + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + TELE.addData("Velocity", velo); + TELE.update(); + + return !flywheel.getSteady(); + } + }; + } + + public Action Obelisk() { + return new Action() { + int id = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + id = fiducial.getFiducialId(); + TELE.addData("ID", id); + TELE.update(); + } + + } + + if (id == 21) { + gpp = true; + } else if (id == 22) { + pgp = true; + } else if (id == 23) { + ppg = true; + } + + TELE.addData("Velocity", velo); + TELE.addData("21", gpp); + TELE.addData("22", pgp); + TELE.addData("23", ppg); + TELE.update(); + + if (gpp || pgp || ppg) { + if (redAlliance) { + robot.limelight.pipelineSwitch(3); + robot.turr1.setPosition(turret_redClose); + robot.turr2.setPosition(1 - turret_redClose); + return false; + + } else { + robot.limelight.pipelineSwitch(2); + double turretPID = turret_blueClose; + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(1 - turretPID); + return false; + } + } else { + return true; + } + } + }; + } + + public Action spindex(double spindexer, int vel) { + return new Action() { + double spinPID = 0.0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + spinPID = servo.setSpinPos(spindexer); + robot.spin1.setPower(spinPID); + robot.spin2.setPower(-spinPID); + TELE.addData("Velocity", velo); + TELE.addLine("spindex"); + TELE.update(); + + drive.updatePoseEstimate(); + teleStart = drive.localizer.getPose(); + + if (servo.spinEqual(spindexer)) { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + + return false; + } else { + return true; + } + } + }; + } + + public Action Shoot(int vel) { + return new Action() { + int ticker = 1; + double initPos = 0.0; + double finalPos = 0.0; + boolean zeroNeeded = false; + boolean zeroPassed = false; + double currentPos = 0.0; + double prevPos = 0.0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + TELE.addData("Velocity", velo); + TELE.addLine("shooting"); + TELE.update(); + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + if (ticker == 1) { + robot.transferServo.setPosition(transferServo_in); + initPos = servo.getSpinPos(); + + finalPos = initPos + 0.6; + + if (finalPos > 1.0) { + finalPos = finalPos - 1; + zeroNeeded = true; + } else if (finalPos > 0.95) { + finalPos = 0.0; + zeroNeeded = true; + } + currentPos = initPos; + } + ticker++; + + if (ticker > 16) { + robot.spin1.setPower(0.08); + robot.spin2.setPower(-0.08); + } + + prevPos = currentPos; + currentPos = servo.getSpinPos(); + if (zeroNeeded) { + if (currentPos - prevPos < -0.5) { + zeroPassed = true; + } + if (zeroPassed) { + if (currentPos > finalPos) { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + return false; + } else { + return true; + } + } else { + return true; + } + } else { + if (currentPos > finalPos) { + robot.spin1.setPower(0); + robot.spin2.setPower(0); + return false; + } else { + return true; + } + } + + } + }; + } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + double spinCurrentPos = 0.0; + double spinInitPos = 0.0; + boolean reverse = false; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + if (ticker % 12 < 3) { + + robot.spin1.setPower(-1); + robot.spin2.setPower(1); + + } else if (reverse) { + robot.spin1.setPower(1); + robot.spin2.setPower(-1); + } else { + robot.spin1.setPower(-0.15); + robot.spin2.setPower(0.15); + } + robot.intake.setPower(1); + TELE.addData("Reverse?", reverse); + TELE.update(); + + if (getRuntime() - stamp > intakeTime) { + if (reverse) { + robot.spin1.setPower(-1); + robot.spin2.setPower(1); + } else { + robot.spin1.setPower(1); + robot.spin2.setPower(-1); + } + return false; + } else { + if (ticker % 4 == 0) { + spinCurrentPos = servo.getSpinPos(); + reverse = Math.abs(spinCurrentPos - spinInitPos) < 0.02; + spinInitPos = spinCurrentPos; + } + return true; + } + } + }; + } + + public Action ColorDetect(int vel) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = getRuntime(); + } + ticker++; + + powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + if (s1D < 43) { + + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + b1 = 2; + } else { + b1 = 1; + } + } + + if (s2D < 60) { + + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + b2 = 2; + } else { + b2 = 1; + } + } + + if (s3D < 33) { + + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; + + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + b3 = 2; + } else { + b3 = 1; + } + } + + TELE.addData("Velocity", velo); + TELE.addLine("Detecting"); + TELE.addData("Distance 1", s1D); + TELE.addData("Distance 2", s2D); + TELE.addData("Distance 3", s3D); + TELE.addData("B1", b1); + TELE.addData("B2", b2); + TELE.addData("B3", b3); + TELE.update(); + + return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); + } + }; + } + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + flywheel = new FlywheelV2(); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + drive = new MecanumDrive(hardwareMap, new Pose2d( + 0, 0, 0 + )); + + servo = new Servos(hardwareMap); + + robot.limelight.pipelineSwitch(1); + robot.limelight.start(); + + TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) + .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); + + TrajectoryActionBuilder lever = drive.actionBuilder(new Pose2d(bx2b, by2b, bh2b)) + .strafeToLinearHeading(new Vector2d(bx2c, by2c), bh2c); + + TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(bx2c, by2c, bh2c)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) + .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); + + TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + TrajectoryActionBuilder pickup3 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx4a, by4a), bh4a) + .strafeToLinearHeading(new Vector2d(bx4b, by4b), bh4b); + TrajectoryActionBuilder shoot3 = drive.actionBuilder(new Pose2d(bx4b, by4b, bh4b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + while (opModeInInit()) { + + if (gamepad2.dpadUpWasPressed()) { + hoodAuto -= 0.01; + } + + if (gamepad2.dpadDownWasPressed()) { + hoodAuto += 0.01; + } + + if (gamepad2.crossWasPressed()) { + redAlliance = !redAlliance; + } + + double turretPID; + if (redAlliance) { + turretPID = turret_redClose; + + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) + .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); + + lever = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) + .strafeToLinearHeading(new Vector2d(rx2c, ry2c), rh2c); + + shoot1 = drive.actionBuilder(new Pose2d(rx2c, ry2c, rh2c)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a) + .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b); + + shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) + .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); + + pickup3 = drive.actionBuilder(new Pose2d(rx1,ry1, rh1)) + .strafeToLinearHeading(new Vector2d(rx4a, ry4a), rh4a) + .strafeToLinearHeading(new Vector2d(rx4b, ry4b), rh4b); + shoot3 = drive.actionBuilder(new Pose2d(bx4b, by4b, bh4b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + } else { + turretPID = turret_blueClose; + + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a) + .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b); + + shoot1 = drive.actionBuilder(new Pose2d(bx2c, by2c, bh2c)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + pickup2 = drive.actionBuilder(new Pose2d(bx1, by1, bh1)) + .strafeToLinearHeading(new Vector2d(bx3a, by3a), bh3a) + .strafeToLinearHeading(new Vector2d(bx3b, by3b), bh3b); + + shoot2 = drive.actionBuilder(new Pose2d(bx3b, by3b, bh3b)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + } + + robot.turr1.setPosition(turretPID); + robot.turr2.setPosition(1 - turretPID); + + robot.hood.setPosition(hoodAuto); + + robot.transferServo.setPosition(transferServo_out); + + TELE.addData("Red?", redAlliance); + TELE.update(); + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + initShooter(AUTO_CLOSE_VEL) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + robot.transfer.setPower(1); + + shootingSequence(); + + robot.transfer.setPower(0); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + intake(intake1Time) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new SequentialAction( + lever.build(), + shoot1.build() + ) + ); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + robot.transfer.setPower(1); + + shootingSequence(); + + robot.transfer.setPower(0); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + intake(intake2Time) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + shoot2.build() + ) + ); + + robot.transfer.setPower(1); + + shootingSequence(); + + robot.transfer.setPower(0); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + intake(intake2Time) + ) + ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + Actions.runBlocking( + new ParallelAction( + shoot3.build() + ) + ); + + robot.transfer.setPower(1); + + shootingSequence(); + + robot.transfer.setPower(0); + + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + TELE.addData("Velocity", velo); + TELE.addLine("finished"); + TELE.update(); + + sleep(2000); + + } + + } + + //TODO: adjust this according to Teleop numbers + public void detectTag() { + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + bearing = result.getTx(); + } + } + double turretPos = robot.turr1Pos.getCurrentPosition() - (bearing / 1300); + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(1 - turretPos); + } + + public void shootingSequence() { + TELE.addLine("Shooting"); + TELE.update(); + Actions.runBlocking(Shoot(AUTO_CLOSE_VEL)); + } + + public void sequence1() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence2() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence3() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence4() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence5() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } + + public void sequence6() { + Actions.runBlocking( + new SequentialAction( + spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL), + spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), + Shoot(AUTO_CLOSE_VEL) + ) + ); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java deleted file mode 100644 index 00ce9a4..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ /dev/null @@ -1,771 +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 static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; - -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.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SequentialAction; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ftc.Actions; - -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; - -@Config -@Autonomous(preselectTeleOp = "TeleopV2") -public class Red_V2 extends LinearOpMode { - - Robot robot; - - MultipleTelemetry TELE; - - MecanumDrive drive; - - AprilTagWebcam aprilTag; - - Flywheel flywheel; - - Servos servo; - - double velo = 0.0; - public static double intake1Time = 2.9; - - public static double intake2Time = 2.9; - - public static double colorDetect = 3.0; - - boolean gpp = false; - - boolean pgp = false; - - boolean ppg = false; - - double powPID = 0.0; - - double bearing = 0.0; - - int b1 = 0; // 0 = no ball, 1 = green, 2 = purple - - int b2 = 0;// 0 = no ball, 1 = green, 2 = purple - - int b3 = 0;// 0 = no ball, 1 = green, 2 = purple - - public Action initShooter(int vel) { - return new Action() { - double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double stamp2 = 0.0; - boolean steady = false; - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp2 = getRuntime(); - } - - ticker++; - if (ticker % 16 == 0) { - stamp = getRuntime(); - stamp1 = stamp; - } - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.transfer.setPower(1); - - TELE.addData("Velocity", velo); - TELE.update(); - if (vel < velo && getRuntime() - stamp2 > 3.0 && !steady){ - steady = true; - stamp2 = getRuntime(); - return true; - } else if (steady && getRuntime() - stamp2 > 1.5){ - TELE.addData("Velocity", velo); - TELE.addLine("finished init"); - TELE.update(); - return false; - } else { - return true; - } - } - }; - } - - public Action steadyShooter(int vel, boolean last) { - return new Action() { - double stamp = 0.0; - boolean steady = false; - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - steady = flywheel.getSteady(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.transfer.setPower(1); - - TELE.addData("Velocity", velo); - TELE.update(); - detectTag(); - - if (last && !steady){ - stamp = getRuntime(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - return false; - } else if (steady) { - stamp = getRuntime(); - return true; - } else { - return true; - } - } - }; - } - - public Action Obelisk() { - return new Action() { - double stamp = getRuntime(); - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if (aprilTag.getTagById(21) != null) { - gpp = true; - } else if (aprilTag.getTagById(22) != null) { - pgp = true; - } else if (aprilTag.getTagById(23) != null) { - ppg = true; - } - aprilTag.update(); - - TELE.addData("Velocity", velo); - TELE.addData("21", gpp); - TELE.addData("22", pgp); - TELE.addData("23", ppg); - TELE.update(); - - if (gpp || pgp || ppg){ - double turretPID = servo.setTurrPos(turret_redClose); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - return !servo.turretEqual(turret_redClose); - } else { - return true; - } - } - }; - } - - public Action spindex (double spindexer, int vel){ - return new Action() { - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - robot.spin1.setPower(spindexer); - robot.spin2.setPower(1-spindexer); - TELE.addData("Velocity", velo); - TELE.addLine("spindex"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - return !servo.spinEqual(spindexer); - } - }; - } - - public Action Shoot(int vel) { - return new Action() { - double transferStamp = 0.0; - int ticker = 1; - boolean transferIn = false; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - drive.updatePoseEstimate(); - detectTag(); - - teleStart = drive.localizer.getPose(); - - if (ticker == 1) { - transferStamp = getRuntime(); - ticker++; - } - if (getRuntime() - transferStamp > waitTransfer && !transferIn) { - robot.transferServo.setPosition(transferServo_in); - TELE.addData("Velocity", velo); - TELE.addData("ticker", ticker); - TELE.update(); - transferIn = true; - return true; - } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){ - robot.transferServo.setPosition(transferServo_out); - TELE.addData("Velocity", velo); - TELE.addLine("shot once"); - TELE.update(); - return false; - } else { - return true; - } - - } - }; - } - - public Action intake(double intakeTime) { - return new Action() { - double position = 0.0; - double stamp = 0.0; - int ticker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.02; - } else { - position = spindexer_intakePos1 - 0.02; - } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - - TELE.addData("Velocity", velo); - TELE.addLine("Intaking"); - TELE.update(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(1); - if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) { - return false; - } else { - return true; - } - } - }; - } - - public Action intakeReject() { - return new Action() { - double stamp = 0.0; - int ticker = 0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if (getRuntime() - stamp < 0.3){ - return true; - }else { - robot.intake.setPower(0); - return false; - } - } - }; - } - - public Action ColorDetect() { - return new Action() { - double stamp = 0.0; - int ticker = 0; - double position = 0.0; - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - if (ticker == 0) { - stamp = getRuntime(); - } - ticker++; - - if ((getRuntime() % 0.3) > 0.15) { - position = spindexer_intakePos1 + 0.02; - } else { - position = spindexer_intakePos1 - 0.02; - } - robot.spin1.setPower(position); - robot.spin2.setPower(1 - position); - - double s1D = robot.color1.getDistance(DistanceUnit.MM); - double s2D = robot.color2.getDistance(DistanceUnit.MM); - double s3D = robot.color3.getDistance(DistanceUnit.MM); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - if (s1D < 40) { - - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - - - if (gP >= 0.4) { - b1 = 2; - } else { - b1 = 1; - } - } - - if (s2D < 40) { - - double green = robot.color2.getNormalizedColors().green; - double red = robot.color2.getNormalizedColors().red; - double blue = robot.color2.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b2 = 2; - } else { - b2 = 1; - } - } - - if (s3D < 30) { - - double green = robot.color3.getNormalizedColors().green; - double red = robot.color3.getNormalizedColors().red; - double blue = robot.color3.getNormalizedColors().blue; - - double gP = green / (green + red + blue); - - if (gP >= 0.4) { - b3 = 2; - } else { - b3 = 1; - } - } - - TELE.addData("Velocity", velo); - TELE.addLine("Detecting"); - TELE.addData("Distance 1", s1D); - TELE.addData("Distance 2", s2D); - TELE.addData("Distance 3", s3D); - TELE.addData("B1", b1); - TELE.addData("B2", b2); - TELE.addData("B3", b3); - TELE.update(); - - return (b1 + b2 + b3 < 4) && !(getRuntime() - stamp > colorDetect); - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - robot = new Robot(hardwareMap); - - flywheel = new Flywheel(); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - drive = new MecanumDrive(hardwareMap, new Pose2d( - 0, 0, 0 - )); - - aprilTag = new AprilTagWebcam(); - - TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - - TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) - .strafeToLinearHeading(new Vector2d(rx2a, ry2a), rh2a) - .strafeToLinearHeading(new Vector2d(rx2b, ry2b), rh2b); - - TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(rx2b, ry2b, rh2b)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - - TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(rx1, ry1, rh1)) - - .strafeToLinearHeading(new Vector2d(rx3a, ry3a), rh3a) - - .strafeToLinearHeading(new Vector2d(rx3b, ry3b), rh3b); - - TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b)) - .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1); - - aprilTag.init(robot, TELE); - - while (opModeInInit()) { - - if (gamepad2.dpadUpWasPressed()) { - hoodAuto-= 0.01; - } - - if (gamepad2.dpadDownWasPressed()) { - hoodAuto += 0.01; - } - - robot.hood.setPosition(hoodAuto); - - robot.transferServo.setPosition(transferServo_out); - - robot.spin1.setPower(spindexer_intakePos1); - robot.spin2.setPower(1 - spindexer_intakePos1); - - aprilTag.update(); - TELE.addData("Velocity", velo); - TELE.addData("Turret Pos", servo.getTurrPos()); - TELE.update(); - } - - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.hood.setPosition(hoodAuto); - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - initShooter(AUTO_CLOSE_VEL), - Obelisk() - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - - robot.hood.setPosition(hoodAuto); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - intake(intake1Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot1.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() - ) - ); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - intake(intake2Time) - ) - ); - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - Actions.runBlocking( - new ParallelAction( - shoot2.build(), - ColorDetect(), - steadyShooter(AUTO_CLOSE_VEL, true), - intakeReject() - - ) - ); - - powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition()); - velo = flywheel.getVelo(); - robot.shooter1.setPower(powPID); - robot.shooter2.setPower(powPID); - - shootingSequence(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - TELE.addData("Velocity", velo); - TELE.addLine("finished"); - TELE.update(); - - sleep(2000); - - } - - } - - public void detectTag(){ - AprilTagDetection d20 = aprilTag.getTagById(20); - AprilTagDetection d24 = aprilTag.getTagById(24); - - if (d20 != null) { - bearing = d20.ftcPose.bearing; - TELE.addData("Bear", bearing); - } - - if (d24 != null) { - bearing = d24.ftcPose.bearing; - TELE.addData("Bear", bearing); - } - double turretPos = servo.getTurrPos() - (bearing / 1300); - double turretPID = servo.setTurrPos(turretPos); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - } - - public void shootingSequence() { - TELE.addData("Velocity", velo); - if (gpp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b2 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b3 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (pgp) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence4(); - TELE.addLine("sequence4"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence3(); - TELE.addLine("sequence3"); - } else if (b2 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } else if (b3 == 2) { - sequence4(); - TELE.addLine("sequence4"); - } - } else { - sequence3(); - TELE.addLine("sequence3"); - } - } else if (ppg) { - if (b1 + b2 + b3 == 4) { - if (b1 == 2 && b2 - b3 == 0) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2 && b1 - b3 == 0) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2 && b1 - b2 == 0) { - sequence1(); - TELE.addLine("sequence1"); - } else { - sequence1(); - TELE.addLine("sequence1"); - } - } else if (b1 + b2 + b3 >= 5) { - if (b1 == 2) { - sequence6(); - TELE.addLine("sequence6"); - } else if (b2 == 2) { - sequence5(); - TELE.addLine("sequence5"); - } else if (b3 == 2) { - sequence1(); - TELE.addLine("sequence1"); - } - } else { - sequence6(); - TELE.addLine("sequence6"); - } - } else { - sequence1(); - TELE.addLine("sequence1"); - } - TELE.update(); - } - - public void sequence1() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence2() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence3() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence4() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence5() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } - - public void sequence6() { - Actions.runBlocking( - new SequentialAction( - spindex(spindexer_outtakeBall3, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall2, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL), - spindex(spindexer_outtakeBall1, AUTO_CLOSE_VEL), - Shoot(AUTO_CLOSE_VEL) - ) - ); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java deleted file mode 100644 index 2c80fab..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -public class blank { -} 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 6deb4ee..f0eebee 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 @@ -17,15 +17,25 @@ public class Poses { public static double rx1 = 45, ry1 = -7, rh1 = 0; public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140); public static double rx2b = 31, ry2b = 32, rh2b = Math.toRadians(140); + + public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); + public static double rx3a = 58, ry3a = 42, rh3a = Math.toRadians(140); public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140); + public static double rx4a = 71, ry4a = 60, rh4a = Math.toRadians(140); + public static double rx4b = 79, ry4b = 79, rh4b = Math.toRadians(140); + public static double bx1 = 45, by1 = 6, bh1 = 0; public static double bx2a = 53, by2a = -7, bh2a = Math.toRadians(-140); public static double bx2b = 23, by2b = -39, bh2b = Math.toRadians(-140); + public static double bx2c = 40, by2c = -50, bh2c = Math.toRadians(-140); + public static double bx3a = 56, by3a = -34, bh3a = Math.toRadians(-140); public static double bx3b = 34, by3b = -58, bh3b = Math.toRadians(-140); + public static double bx4a = 69, by4a = -60, bh4a = Math.toRadians(-140); + public static double bx4b = 75, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); 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 index 8751327..913167c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -40,6 +40,6 @@ public class ServoPositions { public static double turret_detectRedClose = 0.2; public static double turret_detectBlueClose = 0.6; - public static double turrDefault = 0; + public static double turrDefault = 0.4; } 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 85a0fb8..087971d 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 @@ -5,7 +5,9 @@ import androidx.annotation.NonNull; import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.*; +import com.acmerobotics.roadrunner.AccelConstraint; +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.Actions; import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.DualNum; import com.acmerobotics.roadrunner.HolonomicController; @@ -14,12 +16,20 @@ import com.acmerobotics.roadrunner.MinVelConstraint; import com.acmerobotics.roadrunner.MotorFeedforward; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.PoseVelocity2dDual; import com.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.ProfileParams; +import com.acmerobotics.roadrunner.Rotation2d; import com.acmerobotics.roadrunner.Time; import com.acmerobotics.roadrunner.TimeTrajectory; import com.acmerobotics.roadrunner.TimeTurn; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TrajectoryBuilderParams; import com.acmerobotics.roadrunner.TurnConstraints; +import com.acmerobotics.roadrunner.Twist2d; +import com.acmerobotics.roadrunner.Twist2dDual; +import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.Encoder; @@ -46,56 +56,15 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.teamcode.libs.RR.messages.PoseMessage; -import java.lang.Math; import java.util.Arrays; import java.util.LinkedList; import java.util.List; @Config public final class MecanumDrive { - public static class Params { - // IMU orientation - // TODO: fill in these values based on - // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting - public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT; - public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = - RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD; - - // drive model parameters - public double inPerTick = 0.001978956; - public double lateralInPerTick = 0.0013863732202094405; - public double trackWidthTicks = 6488.883015684446; - - // feedforward parameters (in tick units) - public double kS = 1.2147826978829488; - public double kV = 0.00032; - public double kA = 0.000046; - - // path profile parameters (in inches) - public double maxWheelVel = 180; - public double minProfileAccel = -40; - public double maxProfileAccel = 180; - - // turn profile parameters (in radians) - public double maxAngVel = 4* Math.PI; // shared with path - public double maxAngAccel = 4* Math.PI; - - // path controller gains - public double axialGain = 4; - public double lateralGain = 4; - public double headingGain = 4; // shared with turn - - public double axialVelGain = 0.0; - public double lateralVelGain = 0.0; - public double headingVelGain = 0.0; // shared with turn - } - public static Params PARAMS = new Params(); - public final MecanumKinematics kinematics = new MecanumKinematics( PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); - public final TurnConstraints defaultTurnConstraints = new TurnConstraints( PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); public final VelConstraint defaultVelConstraint = @@ -105,117 +74,15 @@ public final class MecanumDrive { )); public final AccelConstraint defaultAccelConstraint = new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); - public final DcMotorEx leftFront, leftBack, rightBack, rightFront; - public final VoltageSensor voltageSensor; - public final LazyImu lazyImu; - public final Localizer localizer; private final LinkedList poseHistory = new LinkedList<>(); - private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); - - public class DriveLocalizer implements Localizer { - public final Encoder leftFront, leftBack, rightBack, rightFront; - public final IMU imu; - - private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; - private Rotation2d lastHeading; - private boolean initialized; - private Pose2d pose; - - public DriveLocalizer(Pose2d pose) { - leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); - leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); - rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); - rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); - - imu = lazyImu.get(); - - // TODO: reverse encoders if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - - this.pose = pose; - } - - @Override - public void setPose(Pose2d pose) { - this.pose = pose; - } - - @Override - public Pose2d getPose() { - return pose; - } - - @Override - public PoseVelocity2d update() { - PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); - PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity(); - PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); - PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); - - YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); - - FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( - leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); - - Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS)); - - if (!initialized) { - initialized = true; - - lastLeftFrontPos = leftFrontPosVel.position; - lastLeftBackPos = leftBackPosVel.position; - lastRightBackPos = rightBackPosVel.position; - lastRightFrontPos = rightFrontPosVel.position; - - lastHeading = heading; - - return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0); - } - - double headingDelta = heading.minus(lastHeading); - Twist2dDual