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

auto rewritten

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-23 20:29:00 -06:00
parent 7a2b275e66
commit 1ae4e1c3ed
2 changed files with 94 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -318,8 +318,13 @@ public class Auto_LT_Close extends LinearOpMode {
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
} }
if (gateCycle) {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1));
} else {
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0));
}
if (gateCycle) { if (gateCycle) {
pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate))) pickup1 = drive.actionBuilder(new Pose2d(xShootGate, yShootGate, Math.toRadians(hShootGate)))
@@ -344,10 +349,17 @@ public class Auto_LT_Close extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
} }
if (gateCycle) {
pickup2 = drive.actionBuilder(new Pose2d(xShoot0, yShoot0, Math.toRadians(hShoot0)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
} else {
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickup1Speed)); new TranslationalVelConstraint(pickup1Speed));
}
if (gateCycle) { if (gateCycle) {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
@@ -368,13 +380,6 @@ public class Auto_LT_Close extends LinearOpMode {
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b))) shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
shoot0ToPickup2 = drive.actionBuilder(new Pose2d(0,0,0))
.strafeToLinearHeading(new Vector2d(xShoot0, yShoot0), Math.toRadians(hShoot0))
.waitSeconds(waitToPickupGate2)
.strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a))
.strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b),
new TranslationalVelConstraint(pickupStackGateSpeed));
if (withPartner) { if (withPartner) {
waitToPickupGate = waitToPickupGateWithPartner; waitToPickupGate = waitToPickupGateWithPartner;
} else { } else {
@@ -407,8 +412,10 @@ public class Auto_LT_Close extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
if (gateCycle) { if (gateCycle) {
shoot0Gate(); startAutoGate();
shoot();
cycleStackMiddleGate(); cycleStackMiddleGate();
shoot();
while (getRuntime() - stamp < endGateTime) { while (getRuntime() - stamp < endGateTime) {
cycleGateIntake(); cycleGateIntake();
@@ -460,12 +467,7 @@ public class Auto_LT_Close extends LinearOpMode {
} }
void shoot() { void shoot() {
Actions.runBlocking( Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease));
new ParallelAction(
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
} }
void startAuto() { void startAuto() {
@@ -485,6 +487,23 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void startAutoGate() {
assert shoot0 != null;
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageShooterAuto(
flywheel0Time,
xShoot0,
yShoot0,
hShoot0,
false
)
)
);
}
void cycleStackClose() { void cycleStackClose() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
@@ -636,59 +655,30 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void shoot0Gate(){ void cycleStackMiddleGate() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot0ToPickup2.build(), pickup2.build(),
new SequentialAction(
new ParallelAction(
autoActions.manageShooterManual(
waitToShoot0,
0.501,
velGate0Start,
hoodGate0Start,
velGate0Start,
hoodGate0Start,
0.501
)
),
autoActions.shootAllManual(
shootAllTime,
hood0MoveTime,
spindexerSpeedIncrease,
velGate0Start,
hoodGate0Start,
velGate0End,
hoodGate0End,
0.501),
autoActions.intake( autoActions.intake(
intake2TimeGate, intake2Time,
xShootGate, x3b,
yShootGate, y3b,
hShootGate h3b
)
) )
) )
); );
}
void cycleStackMiddleGate(){
servos.setSpinPos(spinStartPos); servos.setSpinPos(spinStartPos);
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot2.build(), shoot2.build(),
new SequentialAction( autoActions.prepareShootAll(
new ParallelAction( colorSenseTime,
autoActions.manageShooterAuto( shoot2Time,
shoot2GateTime, motif,
xShootGate, xShootGate,
yShootGate, yShootGate,
hShootGate, hShootGate)
false
)
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
) )
); );
} }
@@ -712,8 +702,6 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
gateCycleShoot.build(), gateCycleShoot.build(),
new SequentialAction(
new ParallelAction(
autoActions.manageShooterAuto( autoActions.manageShooterAuto(
shootGateTime, shootGateTime,
xShootGate, xShootGate,
@@ -721,9 +709,6 @@ public class Auto_LT_Close extends LinearOpMode {
hShootGate, hShootGate,
false false
) )
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
) )
); );
} }
@@ -747,8 +732,6 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot1.build(), shoot1.build(),
new SequentialAction(
new ParallelAction(
autoActions.manageShooterAuto( autoActions.manageShooterAuto(
shoot1GateTime, shoot1GateTime,
xLeaveGate, xLeaveGate,
@@ -756,9 +739,6 @@ public class Auto_LT_Close extends LinearOpMode {
hLeaveGate, hLeaveGate,
false false
) )
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
) )
); );
} }

1
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -131,7 +131,6 @@ public class Turret {
} }
if (xPos != null){ if (xPos != null){
if (zPos>0) { if (zPos>0) {
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ); limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);