diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index 86dbb2a..56376fe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -208,7 +208,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); @@ -272,7 +272,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java index bc1a211..128c67e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java @@ -326,7 +326,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); @@ -390,7 +390,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java deleted file mode 100644 index 89b7da7..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java +++ /dev/null @@ -1,804 +0,0 @@ -package org.firstinspires.ftc.teamcode.autonomous; - -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bHGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShoot1Tangent; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bXGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bYGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.bh1; -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.rHGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShoot1Tangent; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rXGate; -import static org.firstinspires.ftc.teamcode.constants.Poses_V2.rYGate; -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 androidx.annotation.NonNull; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.dashboard.telemetry.TelemetryPacket; -import com.acmerobotics.roadrunner.Action; -import com.acmerobotics.roadrunner.ParallelAction; -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -import com.acmerobotics.roadrunner.TranslationalVelConstraint; -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 com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.Flywheel; -import org.firstinspires.ftc.teamcode.utils.Robot; -import org.firstinspires.ftc.teamcode.utils.Servos; -import org.firstinspires.ftc.teamcode.utils.Spindexer; -import org.firstinspires.ftc.teamcode.utils.Targeting; -import org.firstinspires.ftc.teamcode.utils.Turret; - -@Autonomous(preselectTeleOp = "TeleopV3") -@Config -public class Auto_LT_Close_GateCycle extends LinearOpMode { - - public static double shoot0Vel = 2300, shoot0Hood = 0.93; - public static double autoSpinStartPos = 0.2; - public static double shoot0SpinSpeedIncrease = 0.014; - - public static double spindexerSpeedIncrease = 0.02; - - public static double obeliskTurrPos = 0.53; - public static double gatePickupTime = 3.0; - public static double shoot1Turr = 0.57; - public static double shoot0XTolerance = 1.0; - public static double turretShootPos = 0.72; - public static double shootAllTime = 1.8; - public static double shoot0Time = 1.6; - public static double intake1Time = 3.0; - public static double flywheel0Time = 3.5; - public static double pickup1Speed = 80.0; - // ---- SECOND SHOT / PICKUP ---- - public static double shoot1Vel = 2300; - public static double shoot1Hood = 0.93; - - public static double shootAllVelocity = 2500; - public static double shootAllHood = 0.78; - // ---- 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; - public static double shootCycleTime = 2.5; - public int motif = 0; - Robot robot; - MultipleTelemetry TELE; - MecanumDrive drive; - Servos servos; - Spindexer spindexer; - Flywheel flywheel; - Turret turret; - Targeting targeting; - Targeting.Settings targetingSettings; - 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; - private double xGate, yGate, hGate; - - private double shoot1Tangent; - - 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.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - turret.manualSetTurret(turretShootPos); - - robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); - - return (System.currentTimeMillis() - stamp) < (time * 1000); - - } - }; - } - - public Action shootAll(int vel, double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = vel; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - flywheel.manageFlywheel(vel); - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - public Action shootAllAuto(double shootTime, double spindexSpeed) { - return new Action() { - int ticker = 1; - - double stamp = 0.0; - - double velo = 0.0; - - int shooterTicker = 0; - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - TELE.addData("Velocity", velo); - TELE.addLine("shooting"); - TELE.update(); - - velo = flywheel.getVelo(); - - drive.updatePoseEstimate(); - - teleStart = drive.localizer.getPose(); - - robot.intake.setPower(-0.3); - - if (ticker == 1) { - stamp = getRuntime(); - } - ticker++; - - robot.intake.setPower(0); - - if (getRuntime() - stamp < shootTime) { - - if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spindexSpeed); - robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); - } - - return true; - - } else { - robot.transferServo.setPosition(transferServo_out); - //spindexPos = spindexer_intakePos1; - - spindexer.resetSpindexer(); - spindexer.processIntake(); - - return false; - - } - - } - }; - } - - 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(); - - spindexer.ballCounterLight(); - - 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, - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - 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++; - - flywheel.manageFlywheel(vel); - robot.hood.setPosition(hoodPos); - - 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 manageShooterAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - 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++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - turret.trackGoal(deltaPose); - - robot.hood.setPosition(targetingSettings.hoodAngle); - - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - 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 manageFlywheelAuto( - double time, - double posX, - double posY, - double posXTolerance, - double posYTolerance - ) { - - 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++; - - double robotX = drive.localizer.getPose().position.x; - double robotY = drive.localizer.getPose().position.y; - - double robotHeading = drive.localizer.getPose().heading.toDouble(); - - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); - - double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - - targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0, false); - - - robot.hood.setPosition(targetingSettings.hoodAngle); - - flywheel.manageFlywheel(targetingSettings.flywheelRPM); - - 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; - - } - }; - } - - @Override - public void runOpMode() throws InterruptedException { - - hardwareMap.get(Servo.class, "light").setPosition(0); - - robot = new Robot(hardwareMap); - - TELE = new MultipleTelemetry( - telemetry, FtcDashboard.getInstance().getTelemetry() - ); - - flywheel = new Flywheel(hardwareMap); - - targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0, 0.0); - - spindexer = new Spindexer(hardwareMap); - - servos = new Servos(hardwareMap); - - robot.limelight.pipelineSwitch(1); - - turret = new Turret(robot, TELE, robot.limelight); - - turret.manualSetTurret(0.4); - - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - TrajectoryActionBuilder shoot0 = null; - TrajectoryActionBuilder pickup1 = null; - TrajectoryActionBuilder shoot1 = null; - TrajectoryActionBuilder gatePickup = null; - TrajectoryActionBuilder shootCycle = null; - - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); - - robot.transferServo.setPosition(transferServo_out); - - robot.light.setPosition(1); - - while (opModeInInit()) { - - 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; - shoot1Tangent = rShoot1Tangent; - - xGate = rXGate; - yGate = rYGate; - hGate = rHGate; - - - } 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; - - shoot1Tangent = bShoot1Tangent; - - xGate = bXGate; - yGate = bYGate; - hGate = bHGate; - } - - 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)) - .setReversed(true) - .splineTo(new Vector2d(x2a, y2a), shoot1Tangent) - .splineToSplineHeading(new Pose2d(xShoot, yShoot, hShoot), shoot1Tangent); - - gatePickup = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(xGate, yGate), hGate); - - shootCycle = drive.actionBuilder(new Pose2d(xGate, yGate, hGate)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); - - } - - waitForStart(); - - if (isStopRequested()) return; - - if (opModeIsActive()) { - - robot.transfer.setPower(1); - - assert shoot0 != null; - - Actions.runBlocking( - new ParallelAction( - shoot0.build(), - manageFlywheel( - shoot0Vel, - shoot0Hood, - flywheel0Time, - x1, - 0.501, - shoot0XTolerance, - 0.501 - ) - - ) - ); - - Actions.runBlocking( - shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) - ); - - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - pickup1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - obelisk1Time, - x2b, - y2c, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos - ) - ) - ); - - motif = turret.detectObelisk(); - - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootAll(shoot1Time) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); - - while (opModeIsActive()) { - - Actions.runBlocking( - new ParallelAction( - gatePickup.build(), - manageShooterAuto( - gatePickupTime, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(gatePickupTime) - - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shootCycleTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootCycle.build(), - prepareShootAll(shootCycleTime) - ) - ); - - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) - - ); - - } - - - } - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 790dca5..299b694 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -98,7 +98,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; - public static double gateIntakeTime = 5.0; + public static double gateIntakeTime = 5.0; //TODO: Increase ig public static double spindexerSpeedIncrease = 0.03; @@ -113,7 +113,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 19; + public static double pickup1Speed = 25; // ---- SECOND SHOT / PICKUP ---- public static double shoot1Vel = 2300; public static double shoot1Hood = 0.93; @@ -137,7 +137,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { public static double firstShootTime = 0.3; public static double shootGateTime = 3.0; - public static double gaitWait = 0.8; + public static double gaitWait = 0.45; public int motif = 0; Robot robot; @@ -321,7 +321,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); @@ -385,7 +385,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { drive.updatePoseEstimate(); - Poses_V2.teleStart = drive.localizer.getPose(); + teleStart = drive.localizer.getPose(); robot.intake.setPower(-0.3); 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 8c37cb8..afaa374 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 @@ -24,7 +24,7 @@ public class Poses { public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140); - public static double rx3b = 38, ry3b = 56, rh3b = Math.toRadians(140.1); + public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1); public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140); public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1); 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 53139fc..f9edf09 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 @@ -10,55 +10,16 @@ public class Poses_V2 { public static double turretHeight = 12; - public static double relativeGoalHeight = goalHeight - turretHeight; + public static double rXGateA = 35.5, rYGateA = 46.5, rHGateA = 2.7; - public static Pose2d goalPose = new Pose2d(-10, 0, 0); - - public static double rx1 = 20, ry1 = 0, rh1 = 0; - 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), rt2c = Math.toRadians(Math.PI/2); - - public static double rXGateA = 27, rYGateA = 56, rHGateA = Math.toRadians(160); - - public static double rXGateB = 40, rYGateB = 43, rHGateB = Math.toRadians(159); + public static double rXGateB = 56, rYGateB = 37, rHGateB = 2.7750735106709836; public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179); - public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); - public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); - - public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140); - 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), 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 = 7, rShootH = Math.toRadians(140); - - public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); - public static double bXGateA = 33, bYGateA = 61, bHGateA = Math.toRadians(165); public static double bXGateB = 33, bYGateB = 61, bHGateB = Math.toRadians(165); public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165); - public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140); - public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140); - - public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140); - public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); - public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - - public static double rShoot1Tangent = Math.toRadians(0); - public static double bShoot1Tangent = Math.toRadians(0); - - - - public static Pose2d teleStart = new Pose2d(0, 0, 0); - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index dbdcb98..124ad25 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -329,6 +329,8 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); +// +// TELE.update(); ticker++; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index d2b29a6..57371af 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -1,60 +1,34 @@ package org.firstinspires.ftc.teamcode.utils; -import android.provider.Settings; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.HardwareMap; public class Targeting { - MultipleTelemetry TELE; - - double cancelOffsetX = 0.0; // was -40.0 - double cancelOffsetY = 0.0; // was 7.0 - double unitConversionFactor = 0.95; - - int tileSize = 24; //inches - public final int TILE_UPPER_QUARTILE = 18; - public final int TILE_LOWER_QUARTILE = 6; - - public double robotInchesX, robotInchesY = 0.0; - - public int robotGridX, robotGridY = 0; - - - public static class Settings { - public double flywheelRPM = 0.0; - public double hoodAngle = 0.0; - - public Settings (double flywheelRPM, double hoodAngle) { - this.flywheelRPM = flywheelRPM; - this.hoodAngle = hoodAngle; - } - } - // Known settings discovered using shooter test. // Keep the fidelity at 1 floor tile for now but we could also half it if more // accuracy is needed. public static final Settings[][] KNOWNTARGETING; + static { KNOWNTARGETING = new Settings[6][6]; // ROW 0 - Closet to the goals - KNOWNTARGETING[0][0] = new Settings (2300.0, 0.93); - KNOWNTARGETING[0][1] = new Settings (2300.0, 0.93); - KNOWNTARGETING[0][2] = new Settings (2500.0, 0.78); - KNOWNTARGETING[0][3] = new Settings (2800.0, 0.68); - KNOWNTARGETING[0][4] = new Settings (3000.0, 0.58); - KNOWNTARGETING[0][5] = new Settings (3000.0, 0.58); + KNOWNTARGETING[0][0] = new Settings(2300.0, 0.93); + KNOWNTARGETING[0][1] = new Settings(2300.0, 0.93); + KNOWNTARGETING[0][2] = new Settings(2500.0, 0.78); + KNOWNTARGETING[0][3] = new Settings(2800.0, 0.68); + KNOWNTARGETING[0][4] = new Settings(3000.0, 0.58); + KNOWNTARGETING[0][5] = new Settings(3000.0, 0.58); // ROW 1 - KNOWNTARGETING[1][0] = new Settings (2300.0, 0.93); - KNOWNTARGETING[1][1] = new Settings (2300.0, 0.93); - KNOWNTARGETING[1][2] = new Settings (2600.0, 0.78); + KNOWNTARGETING[1][0] = new Settings(2300.0, 0.93); + KNOWNTARGETING[1][1] = new Settings(2300.0, 0.93); + KNOWNTARGETING[1][2] = new Settings(2600.0, 0.78); // KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62); // KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55); // KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50); - KNOWNTARGETING[1][3] = new Settings (2800.0, 0.68); // Real settings replaced with (0,3) new Settings (2800.0, 0.62); - KNOWNTARGETING[1][4] = new Settings (3000.0, 0.58); // Real setting replaced with (0,4) new Settings (3000.0, 0.55); - KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50); + KNOWNTARGETING[1][3] = new Settings(2800.0, 0.68); // Real settings replaced with (0,3) new Settings (2800.0, 0.62); + KNOWNTARGETING[1][4] = new Settings(3000.0, 0.58); // Real setting replaced with (0,4) new Settings (3000.0, 0.55); + KNOWNTARGETING[1][5] = new Settings(3200.0, 0.50); // ROW 2 // KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78); // KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78); @@ -62,37 +36,47 @@ public class Targeting { // KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53); // KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50); // KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50); - KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78); - KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78); - KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60); - KNOWNTARGETING[2][3] = new Settings (2800.0, 0.62); // Real settings replaced with (1,3) new Settings (2900.0, 0.53); - KNOWNTARGETING[2][4] = new Settings (3000.0, 0.55); // real settings replaces with (1,4) new Settings (3100.0, 0.50); - KNOWNTARGETING[2][5] = new Settings (3200.0, 0.50); // real settings replaced with (1,5) new Settings (3100.0, 0.50); + KNOWNTARGETING[2][0] = new Settings(2500.0, 0.78); + KNOWNTARGETING[2][1] = new Settings(2500.0, 0.78); + KNOWNTARGETING[2][2] = new Settings(2700.0, 0.60); + KNOWNTARGETING[2][3] = new Settings(2800.0, 0.62); // Real settings replaced with (1,3) new Settings (2900.0, 0.53); + KNOWNTARGETING[2][4] = new Settings(3000.0, 0.55); // real settings replaces with (1,4) new Settings (3100.0, 0.50); + KNOWNTARGETING[2][5] = new Settings(3200.0, 0.50); // real settings replaced with (1,5) new Settings (3100.0, 0.50); // ROW 3 - KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50); - KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50); - KNOWNTARGETING[3][2] = new Settings (2900.0, 0.50); - KNOWNTARGETING[3][3] = new Settings (3100.0, 0.47); - KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47); - KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); + KNOWNTARGETING[3][0] = new Settings(2900.0, 0.50); + KNOWNTARGETING[3][1] = new Settings(2900.0, 0.50); + KNOWNTARGETING[3][2] = new Settings(2900.0, 0.50); + KNOWNTARGETING[3][3] = new Settings(3100.0, 0.47); + KNOWNTARGETING[3][4] = new Settings(3100.0, 0.47); + KNOWNTARGETING[3][5] = new Settings(3100.0, 0.47); // ROW 4 - KNOWNTARGETING[4][0] = new Settings (3100, 0.49); - KNOWNTARGETING[4][1] = new Settings (3100, 0.49); - KNOWNTARGETING[4][2] = new Settings (3100, 0.5); - KNOWNTARGETING[4][3] = new Settings (3200, 0.5); - KNOWNTARGETING[4][4] = new Settings (3250, 0.49); - KNOWNTARGETING[4][5] = new Settings (3300, 0.49); + KNOWNTARGETING[4][0] = new Settings(3100, 0.49); + KNOWNTARGETING[4][1] = new Settings(3100, 0.49); + KNOWNTARGETING[4][2] = new Settings(3100, 0.5); + KNOWNTARGETING[4][3] = new Settings(3200, 0.5); + KNOWNTARGETING[4][4] = new Settings(3250, 0.49); + KNOWNTARGETING[4][5] = new Settings(3300, 0.49); // ROW 5 - KNOWNTARGETING[5][0] = new Settings (3200, 0.48); - KNOWNTARGETING[5][1] = new Settings (3200, 0.48); - KNOWNTARGETING[5][2] = new Settings (3300, 0.48); - KNOWNTARGETING[5][3] = new Settings (3350, 0.48); - KNOWNTARGETING[5][4] = new Settings (3350, 0.48); - KNOWNTARGETING[5][5] = new Settings (3350, 0.48); + KNOWNTARGETING[5][0] = new Settings(3200, 0.48); + KNOWNTARGETING[5][1] = new Settings(3200, 0.48); + KNOWNTARGETING[5][2] = new Settings(3300, 0.48); + KNOWNTARGETING[5][3] = new Settings(3350, 0.48); + KNOWNTARGETING[5][4] = new Settings(3350, 0.48); + KNOWNTARGETING[5][5] = new Settings(3350, 0.48); + } - public Targeting() - { + public final int TILE_UPPER_QUARTILE = 18; + public final int TILE_LOWER_QUARTILE = 6; + public double robotInchesX, robotInchesY = 0.0; + public int robotGridX, robotGridY = 0; + MultipleTelemetry TELE; + double cancelOffsetX = 0.0; // was -40.0 + double cancelOffsetY = 0.0; // was 7.0 + double unitConversionFactor = 0.95; + int tileSize = 24; //inches + + public Targeting() { } public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { @@ -111,8 +95,8 @@ public class Targeting { int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); - int remX = Math.floorMod((int)robotInchesX, tileSize); - int remY = Math.floorMod((int)robotInchesX, tileSize); + int remX = Math.floorMod((int) robotInchesX, tileSize); + int remY = Math.floorMod((int) robotInchesX, tileSize); // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. @@ -122,7 +106,7 @@ public class Targeting { int y1 = 0; interpolate = false; if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && - (robotGridX < 5) && (robotGridY <5)) { + (robotGridX < 5) && (robotGridY < 5)) { // +X, +Y interpolate = true; x0 = robotGridX; @@ -186,12 +170,18 @@ public class Targeting { } //clamp - robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); - robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + + if (redAlliance) { + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + } else { + robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + } // basic search - if(true) { //!interpolate) { - if ((robotGridY < 6) && (robotGridX <6)) { + if (true) { //!interpolate) { + if ((robotGridY < 6) && (robotGridX < 6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; } @@ -220,9 +210,19 @@ public class Targeting { // recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; // recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; // Average target tiles - recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0; - recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0; + recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM) / 2.0; + recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle) / 2.0; return recommendedSettings; } } + + public static class Settings { + public double flywheelRPM = 0.0; + public double hoodAngle = 0.0; + + public Settings(double flywheelRPM, double hoodAngle) { + this.flywheelRPM = flywheelRPM; + this.hoodAngle = hoodAngle; + } + } }