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

untested edits of autos

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-27 19:01:33 -06:00
parent c01edd9308
commit c42fce2e78
3 changed files with 185 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -134,6 +134,7 @@ public class Auto_LT_Close extends LinearOpMode {
double obeliskTurrPos3 = 0.0; double obeliskTurrPos3 = 0.0;
double waitToPickupGate = 0; double waitToPickupGate = 0;
double obeliskTurrPosAutoStart = 0; double obeliskTurrPosAutoStart = 0;
boolean limelightStart = false;
// initialize path variables here // initialize path variables here
TrajectoryActionBuilder shoot0 = null; TrajectoryActionBuilder shoot0 = null;
@@ -185,7 +186,7 @@ public class Auto_LT_Close extends LinearOpMode {
hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
while (opModeInInit()) { while (opModeInInit()) {
if (limelightUsed && !gateCycle){ if (limelightUsed && !gateCycle && limelightStart){
Actions.runBlocking( Actions.runBlocking(
autoActions.detectObelisk( autoActions.detectObelisk(
0.1, 0.1,
@@ -207,14 +208,6 @@ public class Auto_LT_Close extends LinearOpMode {
} }
} }
if (!gateCycle) {
turret.pipelineSwitch(1);
} else if (redAlliance) {
turret.pipelineSwitch(4);
} else {
turret.pipelineSwitch(2);
}
if (gateCycle) { if (gateCycle) {
servos.setHoodPos(hoodGate0Start); servos.setHoodPos(hoodGate0Start);
} else { } else {
@@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode {
if (gamepad2.squareWasPressed()) { if (gamepad2.squareWasPressed()) {
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
sleep(100);
robot.limelight.start(); robot.limelight.start();
limelightUsed = true; limelightUsed = true;
limelightStart = true;
gamepad2.rumble(500); gamepad2.rumble(500);
} }
@@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode {
if (redAlliance) { if (redAlliance) {
robot.light.setPosition(0.28); robot.light.setPosition(0.28);
if (gateCycle){
turret.pipelineSwitch(1);
} else {
turret.pipelineSwitch(4);
}
// ---- FIRST SHOT ---- // ---- FIRST SHOT ----
x1 = rx1; x1 = rx1;
y1 = ry1; y1 = ry1;
@@ -314,6 +315,12 @@ public class Auto_LT_Close extends LinearOpMode {
} else { } else {
robot.light.setPosition(0.6); robot.light.setPosition(0.6);
if (gateCycle){
turret.pipelineSwitch(5);
} else {
turret.pipelineSwitch(2);
}
// ---- FIRST SHOT ---- // ---- FIRST SHOT ----
x1 = bx1; x1 = bx1;
y1 = by1; y1 = by1;

210
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java
View File

@@ -12,6 +12,9 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObelisk
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redTurretShootPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -57,12 +60,17 @@ public class Auto_LT_Far extends LinearOpMode {
AutoActions autoActions; AutoActions autoActions;
Light light; Light light;
double xShoot, yShoot, hShoot; double xShoot, yShoot, hShoot;
double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0; double pickupGateXA = 0, pickupGateYA = 0, pickupGateHA = 0;
double pickupGateXB = 0, pickupGateYB = 0, pickupGateHB = 0;
double pickupGateXC = 0, pickupGateYC = 0, pickupGateHC = 0;
public static double flywheel0Time = 1.5; public static double flywheel0Time = 1.5;
boolean gatePickup = true; boolean gatePickup = true;
boolean stack3 = true; boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA; boolean stack2 = true;
double xStackPickupB, yStackPickupB, hStackPickupB; double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA;
double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB;
double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA;
double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB;
public static int pickupStackSpeed = 17; public static int pickupStackSpeed = 17;
public static int pickupGateSpeed = 25; public static int pickupGateSpeed = 25;
int prevMotif = 0; int prevMotif = 0;
@@ -88,6 +96,7 @@ public class Auto_LT_Far extends LinearOpMode {
TrajectoryActionBuilder shoot3 = null; TrajectoryActionBuilder shoot3 = null;
TrajectoryActionBuilder pickupGate = null; TrajectoryActionBuilder pickupGate = null;
TrajectoryActionBuilder shootGate = null; TrajectoryActionBuilder shootGate = null;
TrajectoryActionBuilder pickup2 = null;
Pose2d autoStart = new Pose2d(0,0,0); Pose2d autoStart = new Pose2d(0,0,0);
@Override @Override
@@ -124,14 +133,40 @@ public class Auto_LT_Far extends LinearOpMode {
servos.setTransferPos(transferServo_out); servos.setTransferPos(transferServo_out);
while (opModeInInit()) { limelightUsed = false;
if (gamepad2.leftBumperWasPressed()){ while (opModeInInit()) {
if (limelightUsed){
Actions.runBlocking(
autoActions.detectObelisk(
0.1,
0.501,
0.501,
0.501,
0.501,
turrDefault
)
);
motif = turret.getObeliskID();
if (motif == 21){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall1;
} else if (motif == 22){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b;
} else {
AutoActions.firstSpindexShootPos = spindexer_outtakeBall2;
}
}
if (gamepad2.triangleWasPressed()){
gatePickup = !gatePickup; gatePickup = !gatePickup;
} }
if (gamepad2.rightBumperWasPressed()){ if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3; stack3 = !stack3;
} }
if (gamepad2.leftBumperWasPressed()){
stack2 = !stack2;
}
turret.setTurret(turretShootPos); turret.setTurret(turretShootPos);
@@ -164,17 +199,33 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = rShootY; yShoot = rShootY;
hShoot = rShootH; hShoot = rShootH;
xStackPickupA = rStackPickupAX; xStackPickupFarA = rStackPickupAX;
yStackPickupA = rStackPickupAY; yStackPickupFarA = rStackPickupAY;
hStackPickupA = rStackPickupAH; hStackPickupFarA = rStackPickupAH;
xStackPickupB = rStackPickupBX; xStackPickupFarB = rStackPickupBX;
yStackPickupB = rStackPickupBY; yStackPickupFarB = rStackPickupBY;
hStackPickupB = rStackPickupBH; hStackPickupFarB = rStackPickupBH;
pickupGateX = rPickupGateX; xStackPickupMiddleA = rStackPickupAX;
pickupGateY = rPickupGateY; yStackPickupMiddleA = rStackPickupAY;
pickupGateH = rPickupGateH; hStackPickupMiddleA = rStackPickupAH;
xStackPickupMiddleB = rStackPickupBX;
yStackPickupMiddleB = rStackPickupBY;
hStackPickupMiddleB = rStackPickupBH;
pickupGateXA = rPickupGateXA;
pickupGateYA = rPickupGateYA;
pickupGateHA = rPickupGateHA;
pickupGateXB = rPickupGateXB;
pickupGateYB = rPickupGateYB;
pickupGateHB = rPickupGateHB;
pickupGateXC = rPickupGateXC;
pickupGateYC = rPickupGateYC;
pickupGateHC = rPickupGateHC;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -182,10 +233,9 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + redTurretShootPos; turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){ if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(4); turret.pipelineSwitch(1);
robot.limelight.start(); robot.limelight.start();
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); limelightUsed = true;
gamepad2.rumble(500); gamepad2.rumble(500);
} }
} else { } else {
@@ -203,17 +253,33 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = bShootY; yShoot = bShootY;
hShoot = bShootH; hShoot = bShootH;
xStackPickupA = bStackPickupAX; xStackPickupFarA = bStackPickupAX;
yStackPickupA = bStackPickupAY; yStackPickupFarA = bStackPickupAY;
hStackPickupA = bStackPickupAH; hStackPickupFarA = bStackPickupAH;
xStackPickupB = bStackPickupBX; xStackPickupFarB = bStackPickupBX;
yStackPickupB = bStackPickupBY; yStackPickupFarB = bStackPickupBY;
hStackPickupB = bStackPickupBH; hStackPickupFarB = bStackPickupBH;
pickupGateX = bPickupGateX; xStackPickupMiddleA = bStackPickupAX;
pickupGateY = bPickupGateY; yStackPickupMiddleA = bStackPickupAY;
pickupGateH = bPickupGateH; hStackPickupMiddleA = bStackPickupAH;
xStackPickupMiddleB = bStackPickupBX;
yStackPickupMiddleB = bStackPickupBY;
hStackPickupMiddleB = bStackPickupBH;
pickupGateXA = bPickupGateXA;
pickupGateYA = bPickupGateYA;
pickupGateHA = bPickupGateHA;
pickupGateXB = bPickupGateXB;
pickupGateYB = bPickupGateYB;
pickupGateHB = bPickupGateHB;
pickupGateXC = bPickupGateXC;
pickupGateYC = bPickupGateYC;
pickupGateHC = bPickupGateHC;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -221,10 +287,9 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + blueTurretShootPos; turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){ if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(2); turret.pipelineSwitch(5);
robot.limelight.start(); robot.limelight.start();
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); limelightUsed = true;
gamepad2.rumble(500); gamepad2.rumble(500);
} }
} }
@@ -236,29 +301,33 @@ public class Auto_LT_Far extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA)) .strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB), .strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB),
new TranslationalVelConstraint(pickupStackSpeed)); new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupB, yStackPickupB, Math.toRadians(hStackPickupB))) pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupMiddleA, yStackPickupMiddleA), Math.toRadians(hStackPickupMiddleA))
.strafeToLinearHeading(new Vector2d(xStackPickupMiddleB, yStackPickupMiddleB), Math.toRadians(hStackPickupMiddleB),
new TranslationalVelConstraint(pickupStackSpeed));
shoot3 = drive.actionBuilder(new Pose2d(xStackPickupFarB, yStackPickupFarB, Math.toRadians(hStackPickupFarB)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) pickupGate = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(pickupGateX, pickupGateY), Math.toRadians(pickupGateH)) .strafeToLinearHeading(new Vector2d(pickupGateXA, pickupGateYA), Math.toRadians(pickupGateHA))
.waitSeconds(0.2) .waitSeconds(0.2)
.strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB)) .strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
.strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC), .strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
new TranslationalVelConstraint(pickupGateSpeed)); new TranslationalVelConstraint(pickupGateSpeed));
shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH))) shootGate = drive.actionBuilder(new Pose2d(pickupGateXC, pickupGateYC, Math.toRadians(pickupGateHC)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
limelightUsed = true;
TELE.addData("Red?", redAlliance); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault); TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gatePickup); TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack?", stack3); TELE.addData("Pickup Stack Far?", stack3);
TELE.addData("Pickup Stack Middle?", stack2);
TELE.addData("Start Position", autoStart); TELE.addData("Start Position", autoStart);
TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift TELE.addData("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update(); TELE.update();
@@ -282,6 +351,11 @@ public class Auto_LT_Far extends LinearOpMode {
shoot(); shoot();
} }
if (stack2){
cycleStackMiddle();
shoot();
}
while (gatePickup && getRuntime() - stamp < endAutoTime){ while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickupBalls(); cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){ if (getRuntime() - stamp > endAutoTime){
@@ -358,14 +432,14 @@ public class Auto_LT_Far extends LinearOpMode {
pickup3.build(), pickup3.build(),
autoActions.intake( autoActions.intake(
intakeStackTime, intakeStackTime,
xStackPickupB, xStackPickupFarB,
yStackPickupB, yStackPickupFarB,
hStackPickupB hStackPickupFarB
), ),
autoActions.detectObelisk( autoActions.detectObelisk(
intakeStackTime, intakeStackTime,
xStackPickupB, xStackPickupFarB,
yStackPickupB, yStackPickupFarB,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
obeliskTurrPos3 obeliskTurrPos3
@@ -393,20 +467,62 @@ public class Auto_LT_Far extends LinearOpMode {
); );
} }
void cycleStackMiddle(){
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
autoActions.intake(
intakeStackTime,
xStackPickupMiddleB,
yStackPickupMiddleB,
hStackPickupMiddleB
),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupMiddleB,
yStackPickupMiddleB,
posXTolerance,
posYTolerance,
obeliskTurrPos2
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
Actions.runBlocking(
new ParallelAction(
shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootStackTime,
motif,
xShoot,
yShoot,
hShoot)
)
);
}
void cycleGatePickupBalls(){ void cycleGatePickupBalls(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickupGate.build(), pickupGate.build(),
autoActions.intake( autoActions.intake(
intakeStackTime, intakeStackTime,
pickupGateX, pickupGateXC,
pickupGateY, pickupGateYC,
pickupGateH pickupGateHC
), ),
autoActions.detectObelisk( autoActions.detectObelisk(
intakeGateTime, intakeGateTime,
pickupGateX, pickupGateXC,
pickupGateY, pickupGateYC,
posXTolerance, posXTolerance,
posYTolerance, posYTolerance,
obeliskTurrPos3 obeliskTurrPos3

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java
View File

@@ -16,12 +16,12 @@ public class Back_Poses {
public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1; public static double rStackPickupBX = 53, rStackPickupBY = 71, rStackPickupBH = 140.1;
public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1; public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140; public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140;
public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140; public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140;
public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140; public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140;
public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190; public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -140;
public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190;
public static double bPickupGateXC = 50, bPickupGateYC = -83, bPickupGateHC = -190;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50; public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;
public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50; public static double autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;