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 waitToPickupGate = 0;
double obeliskTurrPosAutoStart = 0;
boolean limelightStart = false;
// initialize path variables here
TrajectoryActionBuilder shoot0 = null;
@@ -185,7 +186,7 @@ public class Auto_LT_Close extends LinearOpMode {
hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
while (opModeInInit()) {
if (limelightUsed && !gateCycle){
if (limelightUsed && !gateCycle && limelightStart){
Actions.runBlocking(
autoActions.detectObelisk(
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) {
servos.setHoodPos(hoodGate0Start);
} else {
@@ -249,8 +242,10 @@ public class Auto_LT_Close extends LinearOpMode {
if (gamepad2.squareWasPressed()) {
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
sleep(100);
robot.limelight.start();
limelightUsed = true;
limelightStart = true;
gamepad2.rumble(500);
}
@@ -258,6 +253,12 @@ public class Auto_LT_Close extends LinearOpMode {
if (redAlliance) {
robot.light.setPosition(0.28);
if (gateCycle){
turret.pipelineSwitch(1);
} else {
turret.pipelineSwitch(4);
}
// ---- FIRST SHOT ----
x1 = rx1;
y1 = ry1;
@@ -314,6 +315,12 @@ public class Auto_LT_Close extends LinearOpMode {
} else {
robot.light.setPosition(0.6);
if (gateCycle){
turret.pipelineSwitch(5);
} else {
turret.pipelineSwitch(2);
}
// ---- FIRST SHOT ----
x1 = bx1;
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.redTurretShootPos;
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.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -57,12 +60,17 @@ public class Auto_LT_Far extends LinearOpMode {
AutoActions autoActions;
Light light;
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;
boolean gatePickup = true;
boolean stack3 = true;
double xStackPickupA, yStackPickupA, hStackPickupA;
double xStackPickupB, yStackPickupB, hStackPickupB;
boolean stack2 = true;
double xStackPickupFarA, yStackPickupFarA, hStackPickupFarA;
double xStackPickupFarB, yStackPickupFarB, hStackPickupFarB;
double xStackPickupMiddleA, yStackPickupMiddleA, hStackPickupMiddleA;
double xStackPickupMiddleB, yStackPickupMiddleB, hStackPickupMiddleB;
public static int pickupStackSpeed = 17;
public static int pickupGateSpeed = 25;
int prevMotif = 0;
@@ -88,6 +96,7 @@ public class Auto_LT_Far extends LinearOpMode {
TrajectoryActionBuilder shoot3 = null;
TrajectoryActionBuilder pickupGate = null;
TrajectoryActionBuilder shootGate = null;
TrajectoryActionBuilder pickup2 = null;
Pose2d autoStart = new Pose2d(0,0,0);
@Override
@@ -124,14 +133,40 @@ public class Auto_LT_Far extends LinearOpMode {
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;
}
if (gamepad2.rightBumperWasPressed()){
stack3 = !stack3;
}
if (gamepad2.leftBumperWasPressed()){
stack2 = !stack2;
}
turret.setTurret(turretShootPos);
@@ -164,17 +199,33 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = rShootY;
hShoot = rShootH;
xStackPickupA = rStackPickupAX;
yStackPickupA = rStackPickupAY;
hStackPickupA = rStackPickupAH;
xStackPickupFarA = rStackPickupAX;
yStackPickupFarA = rStackPickupAY;
hStackPickupFarA = rStackPickupAH;
xStackPickupB = rStackPickupBX;
yStackPickupB = rStackPickupBY;
hStackPickupB = rStackPickupBH;
xStackPickupFarB = rStackPickupBX;
yStackPickupFarB = rStackPickupBY;
hStackPickupFarB = rStackPickupBH;
pickupGateX = rPickupGateX;
pickupGateY = rPickupGateY;
pickupGateH = rPickupGateH;
xStackPickupMiddleA = rStackPickupAX;
yStackPickupMiddleA = rStackPickupAY;
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;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
@@ -182,10 +233,9 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + redTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(4);
turret.pipelineSwitch(1);
robot.limelight.start();
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
limelightUsed = true;
gamepad2.rumble(500);
}
} else {
@@ -203,17 +253,33 @@ public class Auto_LT_Far extends LinearOpMode {
yShoot = bShootY;
hShoot = bShootH;
xStackPickupA = bStackPickupAX;
yStackPickupA = bStackPickupAY;
hStackPickupA = bStackPickupAH;
xStackPickupFarA = bStackPickupAX;
yStackPickupFarA = bStackPickupAY;
hStackPickupFarA = bStackPickupAH;
xStackPickupB = bStackPickupBX;
yStackPickupB = bStackPickupBY;
hStackPickupB = bStackPickupBH;
xStackPickupFarB = bStackPickupBX;
yStackPickupFarB = bStackPickupBY;
hStackPickupFarB = bStackPickupBH;
pickupGateX = bPickupGateX;
pickupGateY = bPickupGateY;
pickupGateH = bPickupGateH;
xStackPickupMiddleA = bStackPickupAX;
yStackPickupMiddleA = bStackPickupAY;
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;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
@@ -221,10 +287,9 @@ public class Auto_LT_Far extends LinearOpMode {
turretShootPos = turrDefault + blueTurretShootPos;
if (gamepad2.squareWasPressed()){
turret.pipelineSwitch(2);
turret.pipelineSwitch(5);
robot.limelight.start();
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
limelightUsed = true;
gamepad2.rumble(500);
}
}
@@ -236,29 +301,33 @@ public class Auto_LT_Far extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(xStackPickupA, yStackPickupA), Math.toRadians(hStackPickupA))
.strafeToLinearHeading(new Vector2d(xStackPickupB, yStackPickupB), Math.toRadians(hStackPickupB),
.strafeToLinearHeading(new Vector2d(xStackPickupFarA, yStackPickupFarA), Math.toRadians(hStackPickupFarA))
.strafeToLinearHeading(new Vector2d(xStackPickupFarB, yStackPickupFarB), Math.toRadians(hStackPickupFarB),
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));
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)
.strafeToLinearHeading(new Vector2d(pickupGateXB, pickupGateYB), Math.toRadians(pickupGateHB))
.strafeToLinearHeading(new Vector2d(pickupGateXC, pickupGateYC), Math.toRadians(pickupGateHC),
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));
limelightUsed = true;
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
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("Current Position", drive.localizer.getPose()); // use this to test standstill drift
TELE.update();
@@ -282,6 +351,11 @@ public class Auto_LT_Far extends LinearOpMode {
shoot();
}
if (stack2){
cycleStackMiddle();
shoot();
}
while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){
@@ -358,14 +432,14 @@ public class Auto_LT_Far extends LinearOpMode {
pickup3.build(),
autoActions.intake(
intakeStackTime,
xStackPickupB,
yStackPickupB,
hStackPickupB
xStackPickupFarB,
yStackPickupFarB,
hStackPickupFarB
),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupB,
yStackPickupB,
xStackPickupFarB,
yStackPickupFarB,
posXTolerance,
posYTolerance,
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(){
Actions.runBlocking(
new ParallelAction(
pickupGate.build(),
autoActions.intake(
intakeStackTime,
pickupGateX,
pickupGateY,
pickupGateH
pickupGateXC,
pickupGateYC,
pickupGateHC
),
autoActions.detectObelisk(
intakeGateTime,
pickupGateX,
pickupGateY,
pickupGateXC,
pickupGateYC,
posXTolerance,
posYTolerance,
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 bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
public static double rPickupGateX = 50, rPickupGateY = 83, rPickupGateH = 140;
public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140;
public static double pickupGateXB = 84, pickupGateYB = 76, pickupGateHB = 140;
public static double pickupGateXC = 50, pickupGateYC = 83, pickupGateHC = 190;
public static double rPickupGateXA = 50, rPickupGateYA = 83, rPickupGateHA = 140;
public static double bPickupGateXA = 70, bPickupGateYA = -90, bPickupGateHA = -140;
public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 140;
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 autoStartBX = 103, autoStartBY = -63.5, autoStartBH = -50;