diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java index ce3098b..7e7f410 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java @@ -1,14 +1,39 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses_V2.*; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bt2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx1; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by1; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.by2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh1; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rt2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx1; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry1; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.ry2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.teleStart; 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.teleop.TeleopV3.spinPow; -import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; -import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spindexPos; import androidx.annotation.NonNull; @@ -20,6 +45,7 @@ import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -38,22 +64,89 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Config public class Auto_LT extends LinearOpMode { - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - - Servos servos; - Spindexer spindexer; - - Flywheel flywheel; - - - private double x1, y1, h1; + public int motif = 0; public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double autoSpinStartPos = 0.2; + public static double shoot0SpinSpeedIncrease = 0.014; + public static double obeliskTurrPos = 0.53; - public static double shoot0SpinSpeedIncrease = 0.03; + public static double shoot1Turr = 0.57; + + public static double shoot0XTolerance = 1.0; + public static double shoot0Time = 1.8; + + public static double intake1Time = 5.0; + + public static double flywheel0Time = 3.5; + public static double pickup1Speed = 20.0; + + // ---- SECOND SHOT / PICKUP ---- + public static double shoot1Vel = 2300; + public static double shoot1Hood = 0.93; + + // ---- PICKUP TIMING ---- + public static double pickup1Time = 3.0; + + // ---- PICKUP POSITION TOLERANCES ---- + public static double pickup1XTolerance = 2.0; + public static double pickup1YTolerance = 2.0; + + // ---- OBELISK DETECTION ---- + public static double obelisk1Time = 1.5; + public static double obelisk1XTolerance = 2.0; + public static double obelisk1YTolerance = 2.0; + + public static double shoot1ToleranceX = 2.0; + public static double shoot1ToleranceY = 2.0; + public static double shoot1Time = 2.0; + + + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Servos servos; + Spindexer spindexer; + Flywheel flywheel; + Turret turret; + private double x1, y1, h1; + + private double x2a, y2a, h2a, t2a; + + private double x2b, y2b, h2b, t2b; + private double x2c, y2c, h2c, t2c; + + private double xShoot, yShoot, hShoot; + + public Action prepareShootAll(double time) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + robot.turr1.setPosition(shoot1Turr); + robot.turr1.setPosition(1-shoot1Turr); + + + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + + robot.transferServo.setPosition(transferServo_out); + + robot.intake.setPower(-((System.currentTimeMillis() - stamp))/1000); + + return (System.currentTimeMillis() - stamp) < (time * 1000); + + + } + }; + } public Action shootAll(int vel, double shootTime) { return new Action() { @@ -89,9 +182,9 @@ public class Auto_LT extends LinearOpMode { if (getRuntime() - stamp < shootTime) { - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)){ + if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1-autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); } else { robot.transferServo.setPosition(transferServo_in); shooterTicker++; @@ -99,14 +192,13 @@ public class Auto_LT extends LinearOpMode { robot.spin1.setPosition(prevSpinPos + shoot0SpinSpeedIncrease); robot.spin2.setPosition(1 - prevSpinPos - shoot0SpinSpeedIncrease); } - + return true; } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; - spindexer.resetSpindexer(); spindexer.processIntake(); @@ -117,6 +209,79 @@ public class Auto_LT extends LinearOpMode { } }; } + + public Action intake(double intakeTime) { + return new Action() { + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + ticker++; + + spindexer.processIntake(); + robot.intake.setPower(1); + + motif = turret.detectObelisk(); + + return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); + + + } + }; + } + + + public Action detectObelisk( + double time, + double posX, + double posY, + double posXTolerance, + double posYTolerance, + double turrPos + ) { + + boolean timeFallback = (time != 0.501); + boolean posXFallback = (posX != 0.501); + boolean posYFallback = (posY != 0.501); + + return new Action() { + + double stamp = 0.0; + int ticker = 0; + + @Override + public boolean run(@NonNull TelemetryPacket telemetryPacket) { + + drive.updatePoseEstimate(); + Pose2d currentPose = drive.localizer.getPose(); + + if (ticker == 0) { + stamp = System.currentTimeMillis(); + } + + ticker++; + + motif = turret.detectObelisk(); + + robot.turr1.setPosition(turrPos); + robot.turr2.setPosition(1 - turrPos); + + boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; + boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; + boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; + + boolean shouldFinish = timeDone || xDone || yDone; + + return !shouldFinish; + + } + }; + } + public Action manageFlywheel( double vel, double hoodPos, @@ -181,20 +346,25 @@ public class Auto_LT extends LinearOpMode { spindexer = new Spindexer(hardwareMap); + servos = new Servos(hardwareMap); + robot.limelight.pipelineSwitch(1); - Turret turret = new Turret(robot, TELE, robot.limelight); + turret = new Turret(robot, TELE, robot.limelight); turret.manualSetTurret(0.4); - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + TrajectoryActionBuilder shoot0; + + TrajectoryActionBuilder pickup1; + TrajectoryActionBuilder shoot1; robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1-autoSpinStartPos); + robot.spin2.setPosition(1 - autoSpinStartPos); + + robot.transferServo.setPosition(transferServo_out); robot.light.setPosition(1); @@ -202,46 +372,101 @@ public class Auto_LT extends LinearOpMode { robot.hood.setPosition(shoot0Hood); - - if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; } if (redAlliance) { robot.light.setPosition(0.28); + + // ---- FIRST SHOT ---- x1 = rx1; y1 = ry1; h1 = rh1; + // ---- PICKUP PATH ---- + x2a = rx2a; + y2a = ry2a; + h2a = rh2a; + t2a = rt2a; + x2b = rx2b; + y2b = ry2b; + h2b = rh2b; + t2b = rt2b; + x2c = rx2c; + y2c = ry2c; + h2c = rh2c; + t2c = rt2c; + + xShoot = rShootX; + yShoot = rShootY; + hShoot = rShootH; + } else { robot.light.setPosition(0.6); + + // ---- FIRST SHOT ---- x1 = bx1; y1 = by1; h1 = bh1; - } + // ---- PICKUP PATH ---- + x2a = bx2a; + y2a = by2a; + h2a = bh2a; + t2a = bt2a; + x2b = bx2b; + y2b = by2b; + h2b = bh2b; + t2b = bt2b; + x2c = bx2c; + y2c = by2c; + h2c = bh2c; + t2c = bt2c; + + xShoot = bShootX; + yShoot = bShootY; + hShoot = bShootH; + } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) .strafeToLinearHeading(new Vector2d(x1, y1), h1); + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) + .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + new TranslationalVelConstraint(pickup1Speed)); + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + } + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) + .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) + .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + new TranslationalVelConstraint(pickup1Speed)); + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); waitForStart(); if (isStopRequested()) return; if (opModeIsActive()) { + robot.transfer.setPower(1); + Actions.runBlocking( new ParallelAction( shoot0.build(), manageFlywheel( shoot0Vel, shoot0Hood, - 3.0, + flywheel0Time, x1, 0.501, - 1, + shoot0XTolerance, 0.501 ) @@ -249,9 +474,63 @@ public class Auto_LT extends LinearOpMode { ); Actions.runBlocking( - shootAll(2300, 3.0) + shootAll((int) shoot0Vel, shoot0Time) ); + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shoot1Vel, + shoot1Hood, + pickup1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + obelisk1Time, + x2b, + y2c, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos + ) + ) + ); + + motif = turret.detectObelisk(); + + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shoot1Vel, + shoot1Hood, + shoot1Time, + xShoot, + yShoot, + shoot1ToleranceX, + shoot1ToleranceY + ), + shoot1.build(), + prepareShootAll(shoot1Time) + ) + ); + + Actions.runBlocking( + shootAll((int) shoot1Vel, shoot0Time) + ); + + + + + TELE.addData("ID", motif); + + TELE.update(); + + robot.intake.setPower(0); sleep(5000); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java index bbe123f..1d597b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java @@ -15,10 +15,10 @@ public class Poses_V2 { public static Pose2d goalPose = new Pose2d(-10, 0, 0); public static double rx1 = 20, ry1 = 0, rh1 = 0; - public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); - public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); + public static double rx2a = 55, ry2a = 39, rh2a = Math.toRadians(140), rt2a = Math.toRadians(Math.PI/2); + public static double rx2b = 33, ry2b = 61, rh2b = Math.toRadians(140), rt2b = Math.toRadians(Math.PI/2); - public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); + public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140), rt2c = Math.toRadians(Math.PI/2); public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); @@ -27,9 +27,13 @@ public class Poses_V2 { public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140); public static double bx1 = 20, by1 = 0, bh1 = 0; - public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); - public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140); - public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140); + public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140), bt2a = Math.toRadians(140); + public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140), bt2b = Math.toRadians(140); + public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140), bt2c = Math.toRadians(140); + + public static double rShootX = 40, rShootY = 30, rShootH = Math.toRadians(140); + + public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140); public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140);