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

Compare commits

base: KeshavAnandCode:7a2b275e66cfe154d3af77ab5d9c1b79a26cd169
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel
..
compare: KeshavAnandCode:2ccd7f04f8df6b96f20038c751804ba6633bc033
Branches Tags
KeshavAnandCode:master KeshavAnandCode:LimelightPID KeshavAnandCode:SpindexerUpgrades KeshavAnandCode:Targeting KeshavAnandCode:SpindexerPosUpdate KeshavAnandCode:SpindexerRefactor KeshavAnandCode:danielv2 KeshavAnandCode:organize/expand KeshavAnandCode:extracontrolhub KeshavAnandCode:daniel

2 Commits
7a2b275e66 ... 2ccd7f04f8

Author SHA1 Message Date
DanTheMan-byte
2ccd7f04f8 put in poses for blue 2026-02-23 21:00:14 -06:00
DanTheMan-byte
1ae4e1c3ed auto rewritten 2026-02-23 20:29:00 -06:00
4 changed files with 105 additions and 126 deletions
Expand all files Collapse all files

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

@@ -167,7 +167,7 @@ public class Auto_LT_Close extends LinearOpMode {
while (opModeInInit()) { while (opModeInInit()) {
if (gateCycle){ if (gateCycle) {
servos.setHoodPos(hoodGate0Start); servos.setHoodPos(hoodGate0Start);
} else { } else {
servos.setHoodPos(shoot0Hood); servos.setHoodPos(shoot0Hood);
@@ -187,17 +187,17 @@ public class Auto_LT_Close extends LinearOpMode {
turrDefault += 0.01; turrDefault += 0.01;
} }
if (gamepad2.rightBumperWasPressed()){ if (gamepad2.rightBumperWasPressed()) {
ballCycles++; ballCycles++;
} }
if (gamepad2.leftBumperWasPressed()){ if (gamepad2.leftBumperWasPressed()) {
ballCycles--; ballCycles--;
} }
if (gamepad2.squareWasPressed()){ if (gamepad2.squareWasPressed()) {
if (!gateCycle){ if (!gateCycle) {
turret.pipelineSwitch(1); turret.pipelineSwitch(1);
} else if (redAlliance){ } else if (redAlliance) {
turret.pipelineSwitch(4); turret.pipelineSwitch(4);
} else { } else {
turret.pipelineSwitch(2); turret.pipelineSwitch(2);
@@ -318,10 +318,15 @@ 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)))
.strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a))
.strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b),
@@ -333,10 +338,10 @@ public class Auto_LT_Close extends LinearOpMode {
new TranslationalVelConstraint(pickup1Speed)); new TranslationalVelConstraint(pickup1Speed));
} }
if (gateCycle){ if (gateCycle) {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate)); .strafeToLinearHeading(new Vector2d(xLeaveGate, yLeaveGate), Math.toRadians(hLeaveGate));
} else if (ballCycles < 2){ } else if (ballCycles < 2) {
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else { } else {
@@ -344,15 +349,22 @@ 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)))
.strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate)); .strafeToLinearHeading(new Vector2d(xShootGate, yShootGate), Math.toRadians(hShootGate));
} else if (ballCycles < 3){ } else if (ballCycles < 3) {
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b)))
.strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave));
} else { } else {
@@ -368,14 +380,7 @@ 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)) if (withPartner) {
.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){
waitToPickupGate = waitToPickupGateWithPartner; waitToPickupGate = waitToPickupGateWithPartner;
} else { } else {
waitToPickupGate = waitToPickupGateSolo; waitToPickupGate = waitToPickupGateSolo;
@@ -406,19 +411,21 @@ 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();
if (getRuntime() - stamp < lastShootTime){ if (getRuntime() - stamp < lastShootTime) {
cycleGateShoot(); cycleGateShoot();
} }
} }
cycleStackCloseIntakeGate(); cycleStackCloseIntakeGate();
if (getRuntime() - stamp < lastShootTime){ if (getRuntime() - stamp < lastShootTime) {
cycleStackCloseShootGate(); cycleStackCloseShootGate();
} }
@@ -426,17 +433,17 @@ public class Auto_LT_Close extends LinearOpMode {
startAuto(); startAuto();
shoot(); shoot();
if (ballCycles > 0){ if (ballCycles > 0) {
cycleStackClose(); cycleStackClose();
shoot(); shoot();
} }
if (ballCycles > 1){ if (ballCycles > 1) {
cycleStackMiddle(); cycleStackMiddle();
shoot(); shoot();
} }
if (ballCycles > 2){ if (ballCycles > 2) {
cycleStackFar(); cycleStackFar();
shoot(); shoot();
} }
@@ -459,13 +466,8 @@ 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,7 +487,24 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void cycleStackClose(){ void startAutoGate() {
assert shoot0 != null;
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageShooterAuto(
flywheel0Time,
xShoot0,
yShoot0,
hShoot0,
false
)
)
);
}
void cycleStackClose() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup1.build(), pickup1.build(),
@@ -515,7 +534,7 @@ public class Auto_LT_Close extends LinearOpMode {
double posX; double posX;
double posY; double posY;
double posH; double posH;
if (ballCycles > 1){ if (ballCycles > 1) {
posX = xShoot; posX = xShoot;
posY = yShoot; posY = yShoot;
posH = hShoot; posH = hShoot;
@@ -540,7 +559,7 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void cycleStackMiddle(){ void cycleStackMiddle() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup2.build(), pickup2.build(),
@@ -570,7 +589,7 @@ public class Auto_LT_Close extends LinearOpMode {
double posX; double posX;
double posY; double posY;
double posH; double posH;
if (ballCycles > 2){ if (ballCycles > 2) {
posX = xShoot; posX = xShoot;
posY = yShoot; posY = yShoot;
posH = hShoot; posH = hShoot;
@@ -594,7 +613,7 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void cycleStackFar(){ void cycleStackFar() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup3.build(), pickup3.build(),
@@ -636,64 +655,35 @@ 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)
)
) )
); );
} }
void cycleGateIntake(){ void cycleGateIntake() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
gateCyclePickup.build(), gateCyclePickup.build(),
@@ -707,13 +697,11 @@ public class Auto_LT_Close extends LinearOpMode {
); );
} }
void cycleGateShoot(){ void cycleGateShoot() {
servos.setSpinPos(spinStartPos); servos.setSpinPos(spinStartPos);
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,14 +709,11 @@ public class Auto_LT_Close extends LinearOpMode {
hShootGate, hShootGate,
false false
) )
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
) )
); );
} }
void cycleStackCloseIntakeGate(){ void cycleStackCloseIntakeGate() {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
pickup1.build(), pickup1.build(),
@@ -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)
)
) )
); );
} }

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

@@ -14,10 +14,10 @@ public class Back_Poses {
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140; public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1; public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1; public static double bStackPickupBX = 55, bStackPickupBY = -73, bStackPickupBH = -140.1;
public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140; public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140; public static double bPickupGateX = 70, bPickupGateY = -90, bPickupGateH = -140;
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;

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

@@ -8,33 +8,33 @@ public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double bx2a = 41, by2a = -18, bh2a = -140; public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double rx2b = 23, ry2b = 36, rh2b = 140.1; public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
public static double bx2b = 19, by2b = -40, bh2b = -140.1; public static double bx2b = 23, by2b = -36, bh2b = -140.1;
public static double rx3a = 55, ry3a = 39, rh3a = 140; public static double rx3a = 55, ry3a = 39, rh3a = 140;
public static double bx3a = 55, by3a = -39, bh3a = -140; public static double bx3a = 55, by3a = -39, bh3a = -140;
public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
public static double bx3aG = 55, by3aG = -43, bh3aG = -140; public static double bx3aG = 60, by3aG = -34, bh3aG = -140;
public static double rx3b = 36, ry3b = 58, rh3b = 140.1; public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
public static double bx3b = 41, by3b = -59, bh3b = -140.1; public static double bx3b = 36, by3b = -58, bh3b = -140.1;
public static double rx4a = 75, ry4a = 53, rh4a = 140; public static double rx4a = 75, ry4a = 53, rh4a = 140;
public static double bx4a = 75, by4a = -53, bh4a = -140; public static double bx4a = 75, by4a = -53, bh4a = -140;
public static double rx4b = 50, ry4b = 78, rh4b = 140.1; public static double rx4b = 50, ry4b = 78, rh4b = 140.1;
public static double bx4b = 47, by4b = -85, bh4b = -140.1; public static double bx4b = 50, by4b = -78, bh4b = -140.1;
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
public static double rShootX = 40, rShootY = 10, rShootH = 50; public static double rShootX = 40, rShootY = 10, rShootH = 50;
public static double bShootX = 40, bShootY = 0, bShootH = -50; public static double bShootX = 40, bShootY = -10, bShootH = -50;
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50; public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
@@ -46,16 +46,16 @@ public class Front_Poses {
public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1; public static double bShoot0X = 40, bShoot0Y = -0.1, bShoot0H = -0.1;
public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90; public static double rShootGateX = 50, rShootGateY = 10, rShootGateH = 90;
public static double bShootGateX = 40, bShootGateY = 1, bShootGateH = -90; public static double bShootGateX = 50, bShootGateY = -10, bShootGateH = -90;
public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55; public static double rLeaveGateX = 40, rLeaveGateY = -7, rLeaveGateH = 55;
public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55; public static double bLeaveGateX = 40, bLeaveGateY = 7, bLeaveGateH = -55;
public static double rPickupGateAX = 24, rPickupGateAY = 50, rPickupGateAH = 140; public static double rPickupGateAX = 24, rPickupGateAY = 50, rPickupGateAH = 140;
public static double bPickupGateAX = 36, bPickupGateAY = -50, bPickupGateAH = -140; public static double bPickupGateAX = 24, bPickupGateAY = -50, bPickupGateAH = -140;
public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180; public static double rPickupGateBX = 38, rPickupGateBY = 68, rPickupGateBH = 180;
public static double bPickupGateBX = 46, bPickupGateBY = -60, bPickupGateBH = -180; public static double bPickupGateBX = 38, bPickupGateBY = -68, bPickupGateBH = -180;
public static Pose2d teleStart = new Pose2d(0, 0, 0); public static Pose2d teleStart = new Pose2d(0, 0, 0);
} }

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

@@ -129,9 +129,8 @@ 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);