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

gate auto coded

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-28 12:02:38 -06:00
parent 6743481440
commit b342c98149
2 changed files with 44 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -90,10 +90,6 @@ public class Auto_LT_Close extends LinearOpMode {
public static double intake1GateTime = 3.3;
public static double lastShootTime = 27;
public static double openGateX = 26;
public static double openGateY = 48;
public static double openGateH = Math.toRadians(155);
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
@@ -125,6 +121,8 @@ public class Auto_LT_Close extends LinearOpMode {
double xShootGate, yShootGate, hShootGate;
double xLeave, yLeave, hLeave;
double xLeaveGate, yLeaveGate, hLeaveGate;
double openGateCloseX = 0, openGateCloseY = 0, openGateCloseH = 0;
double openGateMiddleX = 0, openGateMiddleY = 0, openGateMiddleH = 0;
int ballCycles = 3;
int prevMotif = 0;
@@ -310,6 +308,14 @@ public class Auto_LT_Close extends LinearOpMode {
pickupGateBY = rPickupGateBY;
pickupGateBH = rPickupGateBH;
openGateCloseX = rOpenGateCloseX;
openGateCloseY = rOpenGateCloseY;
openGateCloseH = rOpenGateCloseH;
openGateMiddleX = rOpenGateMiddleX;
openGateMiddleY = rOpenGateMiddleY;
openGateMiddleH = rOpenGateMiddleH;
obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -372,25 +378,34 @@ public class Auto_LT_Close extends LinearOpMode {
pickupGateBY = bPickupGateBY;
pickupGateBH = bPickupGateBH;
openGateCloseX = bOpenGateCloseX;
openGateCloseY = bOpenGateCloseY;
openGateCloseH = bOpenGateCloseH;
openGateMiddleX = bOpenGateMiddleX;
openGateMiddleY = bOpenGateMiddleY;
openGateMiddleH = bOpenGateMiddleH;
obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
}
if (gateCycle) {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
} else {
// if (gateCycle) {
// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
// .strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
// } else {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
}
// }
if (gateCycle) {
pickup2 = shoot0.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
new TranslationalVelConstraint(pickupStackGateSpeed))
.strafeToLinearHeading(new Vector2d(openGateMiddleX, openGateMiddleY), Math.toRadians(openGateMiddleH));
} else {
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
@@ -398,13 +413,14 @@ public class Auto_LT_Close extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed));
}
if (gateCycle&& withPartner) {
// if (gateCycle && withPartner) {
// shoot2 = pickup2.endTrajectory().fresh()
// .strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH))
// .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
// } else
if (gateCycle) {
shoot2 = pickup2.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(openGateX, openGateY), Math.toRadians(openGateH))
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(pickupGateAH));
} else if (gateCycle) {
shoot2 = pickup2.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else if (ballCycles < 3) {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
@@ -426,10 +442,11 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
pickup1 = gateCycleShoot.endTrajectory().fresh()
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
new TranslationalVelConstraint(pickupStackGateSpeed));
new TranslationalVelConstraint(pickupStackGateSpeed))
.strafeToLinearHeading(new Vector2d(openGateCloseX, openGateCloseY), Math.toRadians(openGateCloseH));
} else {
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
@@ -440,7 +457,7 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
shoot1 = pickup1.endTrajectory().fresh()
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
} else if (ballCycles < 2) {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
@@ -488,28 +505,11 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
startAutoGate();
shoot(0.501, 0.501, 0.501);
cycleStackMiddleGate();
cycleStackClose();
shoot(0.501,0.501, 0.501);
while (getRuntime() - stamp < endGateTime) {
cycleGateIntake();
if (getRuntime() - stamp < lastShootTime) {
cycleGateShoot();
cycleStackMiddle();
shoot(0.501, 0.501, 0.501);
}
}
cycleStackCloseIntakeGate();
if (getRuntime() - stamp < lastShootTime) {
cycleStackCloseShootGate();
}
shoot(0.501, 0.501, 0.501);
} else {
startAuto();
shoot(0.501, 0.501,0.501);

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -58,5 +58,11 @@ public class Front_Poses {
public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
public static double pickupGateCX = 34, pickupGateCY = 58, pickupGateCH = 220;
public static double rOpenGateCloseX = 20, rOpenGateCloseY = 35, rOpenGateCloseH = 230;
public static double bOpenGateCloseX = 20, bOpenGateCloseY = -35, bOpenGateCloseH = -230;
public static double rOpenGateMiddleX = 36, rOpenGateMiddleY = 59, rOpenGateMiddleH = 50;
public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -500;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}