diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java index c32af9e..cab8505 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java @@ -1,51 +1,51 @@ 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.Poses_LT_Indexed.bHGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c; +import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -58,6 +58,7 @@ 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.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; @@ -74,6 +75,9 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; import org.firstinspires.ftc.teamcode.utils.Turret; +import java.util.ArrayList; +import java.util.List; + @Autonomous(preselectTeleOp = "TeleopV3") @Config public class Auto_LT_Indexed extends LinearOpMode { @@ -93,11 +97,14 @@ public class Auto_LT_Indexed extends LinearOpMode { public static double shoot0Time = 1.6; public static double intake1Time = 3.0; public static double flywheel0Time = 3.5; - public static double pickup1Speed = 80.0; + public static double pickup1Speed = 20.0; + public static double pickup2Speed = 20.0; + public static double pickup3Speed = 20.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 ---- @@ -113,6 +120,11 @@ public class Auto_LT_Indexed extends LinearOpMode { public static double shoot1ToleranceY = 2.0; public static double shoot1Time = 2.0; public static double shootCycleTime = 2.5; + + List s1 = new ArrayList<>(); + + List s2 = new ArrayList<>(); + List s3 = new ArrayList<>(); public int motif = 0; Robot robot; MultipleTelemetry TELE; @@ -135,11 +147,32 @@ public class Auto_LT_Indexed extends LinearOpMode { private double shoot1Tangent; - public Action prepareShootAll(double time) { + // ---- OPEN GATE / MIDFIELD ---- + private double x3, y3, h3, t3; + + // ---- PICKUP 2 ---- + private double x4a, y4a, h4a; + private double x4b, y4b, h4b; + + // ---- PICKUP 3 ---- + private double x5a, y5a, h5a; + private double x5b, y5b, h5b; + + // ---- PARK ---- + private double xPark, yPark, hPark; + + public Action prepareShootIndexed(double time) { return new Action() { double stamp = 0.0; int ticker = 0; + boolean testColor = false; + + int s1Green = 0; + int s2Green = 0; + int s3Green = 0; + + @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { if (ticker == 0) { @@ -147,8 +180,56 @@ public class Auto_LT_Indexed extends LinearOpMode { } ticker++; - robot.spin1.setPosition(autoSpinStartPos); - robot.spin2.setPosition(1 - autoSpinStartPos); + if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish??? + robot.spin1.setPosition(colorDetectPos); //0.43 + robot.spin2.setPosition(1 - colorDetectPos); + } else if ((System.currentTimeMillis() - stamp) < (colorSenseTime + goToDetectTime)) { + + + double green1 = robot.color1.getNormalizedColors().green; + double red1 = robot.color1.getNormalizedColors().red; + double blue1 = robot.color1.getNormalizedColors().blue; + + double gP1 = green1 / (green1 + red1 + blue1); + + + if (gP1 >= color1Thresh) { + s1Green ++; + } + + double green2 = robot.color2.getNormalizedColors().green; + double red2 = robot.color2.getNormalizedColors().red; + double blue2 = robot.color2.getNormalizedColors().blue; + + double gP2 = green2 / (green2 + red2 + blue2); + + if (gP2 >= color2Thresh) { + s2Green ++; + } + + + double green3 = robot.color3.getNormalizedColors().green; + double red3 = robot.color3.getNormalizedColors().red; + double blue3 = robot.color3.getNormalizedColors().blue; + + double gP3 = green3 / (green3 + red3 + blue3); + + if (gP3 >= color3Thresh) { + s3Green ++; + } + + + testColor = true; + + + + } else { + + double spindexPos; + if (motif == 21) { + spindexPos = + } + } robot.transferServo.setPosition(transferServo_out); @@ -301,7 +382,7 @@ public class Auto_LT_Indexed extends LinearOpMode { robot.intake.setPower(1); motif = turret.detectObelisk(); - + spindexer.ballCounterLight(); return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); @@ -513,7 +594,6 @@ public class Auto_LT_Indexed extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, false); - robot.hood.setPosition(targetingSettings.hoodAngle); flywheel.manageFlywheel(targetingSettings.flywheelRPM); @@ -559,10 +639,15 @@ public class Auto_LT_Indexed extends LinearOpMode { drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); TrajectoryActionBuilder shoot0 = null; + TrajectoryActionBuilder pickup1 = null; TrajectoryActionBuilder shoot1 = null; - TrajectoryActionBuilder gatePickup = null; - TrajectoryActionBuilder shootCycle = null; + TrajectoryActionBuilder pickup2 = null; + TrajectoryActionBuilder shoot2 = null; + TrajectoryActionBuilder pickup3 = null; + TrajectoryActionBuilder shoot3 = null; + TrajectoryActionBuilder openGate = null; + TrajectoryActionBuilder park = null; robot.spin1.setPosition(autoSpinStartPos); robot.spin2.setPosition(1 - autoSpinStartPos); @@ -582,63 +667,69 @@ public class Auto_LT_Indexed extends LinearOpMode { if (redAlliance) { robot.light.setPosition(0.28); - // ---- FIRST SHOT ---- + // ===== FIRST SHOT ===== x1 = rx1; y1 = ry1; h1 = rh1; - // ---- PICKUP PATH ---- + // ===== 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; + // ===== SHOOT POSE ===== xShoot = rShootX; yShoot = rShootY; hShoot = rShootH; shoot1Tangent = rShoot1Tangent; - + + // ===== GATE ===== xGate = rXGate; yGate = rYGate; hGate = rHGate; - } else { robot.light.setPosition(0.6); - // ---- FIRST SHOT ---- + // ===== FIRST SHOT ===== x1 = bx1; y1 = by1; h1 = bh1; - // ---- PICKUP PATH ---- + // ===== 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; + // ===== SHOOT POSE ===== xShoot = bShootX; yShoot = bShootY; hShoot = bShootH; - shoot1Tangent = bShoot1Tangent; + // ===== GATE ===== xGate = bXGate; yGate = bYGate; hGate = bHGate; @@ -651,17 +742,30 @@ public class Auto_LT_Indexed extends LinearOpMode { .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)) + openGate = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) + .setReversed(true) + .splineToConstantHeading(new Vector2d(x3, y3), t3); + + shoot1 = drive.actionBuilder(new Pose2d(x3, y3, h2b)) .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) + .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) + .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, + new TranslationalVelConstraint(pickup2Speed)); + shoot2 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) + .strafeToLinearHeading(new Vector2d(x5a, y5a), h5a) + .strafeToLinearHeading(new Vector2d(x5b, y5b), h5b, + new TranslationalVelConstraint(pickup3Speed)); + shoot3 = drive.actionBuilder(new Pose2d(x5b, y5b, h5b)) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + park = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) + .strafeToLinearHeading(new Vector2d(xPark, yPark), hPark); + } waitForStart(); @@ -694,36 +798,46 @@ public class Auto_LT_Indexed extends LinearOpMode { shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) ); + //WORKING SHOOT ALL for the preload + + //Pickup first pile + Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - pickup1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance + new SequentialAction( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + pickup1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + obelisk1Time, + x2b, + y2c, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos + ) ), - intake(intake1Time), - detectObelisk( - obelisk1Time, - x2b, - y2c, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos + new ParallelAction( + openGate.build() //TODO: Add other managing stuff here ) ) + ); - motif = turret.detectObelisk(); + motif = turret.detectObelisk(); //Detect Obelisk if not alr Actions.runBlocking( new ParallelAction( manageFlywheel( - shootAllVelocity, + shootAllVelocity, shootAllHood, shoot1Time, 0.501, @@ -732,7 +846,7 @@ public class Auto_LT_Indexed extends LinearOpMode { 0.501 ), shoot1.build(), - prepareShootAll(shoot1Time) + prepareShootIndexed(shoot1Time) ) ); @@ -745,58 +859,12 @@ public class Auto_LT_Indexed extends LinearOpMode { 0.501, 0.501 ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) + shootAllIndexed(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) - ) - - ); - - } - + //Shoot from first pile } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_LT_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_LT_Indexed.java new file mode 100644 index 0000000..c9329f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_LT_Indexed.java @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Poses_LT_Indexed { + + // ================= FIELD / GOAL ================= + public static double goalHeight = 42; // inches + public static double turretHeight = 12; + public static double relativeGoalHeight = goalHeight - turretHeight; + + public static Pose2d goalPose = new Pose2d(-10, 0, 0); + public static Pose2d teleStart = new Pose2d(0, 0, 0); + + // ================================================= + // ================= RED ALLIANCE ================== + // ================================================= + + // -------- FIRST SHOT -------- + public static double rx1 = 20, ry1 = 0, rh1 = 0; + + // -------- PICKUP 1 -------- + 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); + + // -------- OPEN GATE -------- + public static double rXGate = 30, rYGate = 63, rHGate = Math.toRadians(179); + + // -------- PICKUP 2 -------- + public static double rx3 = 0, ry3 = 0, rh3 = 0, rt3 = 0; + + public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); + public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); + + // -------- PICKUP 3 -------- + 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 rx5a = 0, ry5a = 0, rh5a = 0; + public static double rx5b = 0, ry5b = 0, rh5b = 0; + + // -------- SHOOT -------- + public static double rShootX = 40, rShootY = 7, rShootH = Math.toRadians(140); + public static double rShoot1Tangent = Math.toRadians(0); + + // -------- PARK -------- + public static double rXPark = 0, rYPark = 0, rHPark = 0; + + // ================================================= + // ================= BLUE ALLIANCE ================= + // ================================================= + + // -------- FIRST SHOT -------- + public static double bx1 = 20, by1 = 0, bh1 = 0; + + // -------- PICKUP 1 -------- + 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); + + // -------- OPEN GATE -------- + public static double bXGate = 25, bYGate = 69, bHGate = Math.toRadians(165); + + // -------- PICKUP 2 -------- + public static double bx3 = 0, by3 = 0, bh3 = 0, bt3 = 0; + + public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140); + public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140); + + // -------- PICKUP 3 -------- + 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 bx5a = 0, by5a = 0, bh5a = 0; + public static double bx5b = 0, by5b = 0, bh5b = 0; + + // -------- SHOOT -------- + public static double bShootX = 20, bShootY = 30, bShootH = Math.toRadians(140); + public static double bShoot1Tangent = Math.toRadians(0); + + // -------- PARK -------- + public static double bXPark = 0, bYPark = 0, bHPark = 0; +} \ No newline at end of file