KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

NOT WORKING indexed auto WIP

Browse Source
This commit is contained in:
Keshav Anand
2026-01-28 17:27:26 -06:00
parent 7a50aa5065
commit 8bc0b1043a
2 changed files with 292 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Indexed.java
View File

@@ -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<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>();
List<Boolean> 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
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_LT_Indexed.java Normal file
View File

@@ -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;
}