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

jytrv

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-31 18:15:10 -06:00
parent b235f9787b
commit b60d64b98f
3 changed files with 47 additions and 305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

342
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Indexed.java
View File

@@ -1,60 +1,7 @@
package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.by1;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.by4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX;
import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx1;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry1;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
@@ -186,6 +133,14 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
private int rearSlotGreen = 0; private int rearSlotGreen = 0;
private int mostGreenSlot = 0; private int mostGreenSlot = 0;
private double pickupGateX = 0, pickupGateY = 0, pickupGateH = 0;
private double pickupZoneX = 0, pickupZoneY = 0, pickupZoneH = 0;
public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1;
public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1;
public static double rShootX = 1, rShootY = 1, rShootH = 1;
public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1;
public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1;
public static double bShootX = 1, bShootY = 1, bShootH = 1;
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
return new Action() { return new Action() {
double stamp = 0.0; double stamp = 0.0;
@@ -310,6 +265,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
}; };
} }
public static int sleepTime = 300;
public Action shootAll(int vel, double shootTime, double spindexSpeed) { public Action shootAll(int vel, double shootTime, double spindexSpeed) {
return new Action() { return new Action() {
int ticker = 1; int ticker = 1;
@@ -762,13 +719,10 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
robot.transferServo.setPosition(transferServo_out); robot.transferServo.setPosition(transferServo_out);
TrajectoryActionBuilder shoot0 = null; TrajectoryActionBuilder shoot0 = null;
TrajectoryActionBuilder pickup1 = null; TrajectoryActionBuilder pickupGate = null;
TrajectoryActionBuilder shoot1 = null; TrajectoryActionBuilder pickupLoadingZone = null;
TrajectoryActionBuilder pickup2 = null; TrajectoryActionBuilder shootFromGate = null;
TrajectoryActionBuilder shoot2 = null; TrajectoryActionBuilder shootFromZone = null;
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
robot.light.setPosition(1); robot.light.setPosition(1);
@@ -805,115 +759,45 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
if (redAlliance) { if (redAlliance) {
robot.light.setPosition(0.28); robot.light.setPosition(0.28);
// ---- FIRST SHOT ---- pickupGateX = rPickupGateX;
x1 = rx1; pickupGateY = rPickupGateY;
y1 = ry1; pickupGateH = rPickupGateH;
h1 = rh1;
pickupZoneX = rPickupZoneX;
pickupZoneY = rPickupZoneY;
pickupZoneH = rPickupZoneH;
// ---- PICKUP PATH ----
x2a = rx2a;
y2a = ry2a;
h2a = rh2a;
x2b = rx2b;
y2b = ry2b;
h2b = rh2b;
x3a = rx3a;
y3a = ry3a;
h3a = rh3a;
x3b = rx3b;
y3b = ry3b;
h3b = rh3b;
x4a = rx4a;
y4a = ry4a;
h4a = rh4a;
x4b = rx4b;
y4b = ry4b;
h4b = rh4b;
xPrep = rxPrep;
yPrep = ryPrep;
hPrep = rhPrep;
xShoot = rShootX; xShoot = rShootX;
yShoot = rShootY; yShoot = rShootY;
hShoot = rShootH; hShoot = rShootH;
obeliskTurrPos1 = redObeliskTurrPos1;
obeliskTurrPos2 = redObeliskTurrPos2;
obeliskTurrPos3 = redObeliskTurrPos3;
turretShootPos = redTurretShootPos; turretShootPos = redTurretShootPos;
} else { } else {
robot.light.setPosition(0.6); robot.light.setPosition(0.6);
// ---- FIRST SHOT ---- pickupGateX = bPickupGateX;
x1 = bx1; pickupGateY = bPickupGateY;
y1 = by1; pickupGateH = bPickupGateH;
h1 = bh1;
// ---- PICKUP PATH ---- pickupZoneX = bPickupZoneX;
x2a = bx2a; pickupZoneY = bPickupZoneY;
y2a = by2a; pickupZoneH = bPickupZoneH;
h2a = bh2a;
x2b = bx2b;
y2b = by2b;
h2b = bh2b;
x3a = bx3a;
y3a = by3a;
h3a = bh3a;
x3b = bx3b;
y3b = by3b;
h3b = bh3b;
x4a = bx4a;
y4a = by4a;
h4a = bh4a;
x4b = bx4b;
y4b = by4b;
h4b = bh4b;
xPrep = bxPrep;
yPrep = byPrep;
hPrep = bhPrep;
xShoot = bShootX; xShoot = bShootX;
yShoot = bShootY; yShoot = bShootY;
hShoot = bShootH; hShoot = bShootH;
obeliskTurrPos1 = blueObeliskTurrPos1;
obeliskTurrPos2 = blueObeliskTurrPos2;
obeliskTurrPos3 = blueObeliskTurrPos3;
turretShootPos = blueTurretShootPos; turretShootPos = blueTurretShootPos;
} }
shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
.strafeToLinearHeading(new Vector2d(x1, y1), h1);
pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2a, y2a), h2a)
.strafeToLinearHeading(new Vector2d(x2b, y2b), h2b,
new TranslationalVelConstraint(pickup1Speed));
shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot))
.strafeToLinearHeading(new Vector2d(x3a, y3a), h3a)
.strafeToLinearHeading(new Vector2d(x3b, y3b), h3b,
new TranslationalVelConstraint(pickup1Speed));
shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x4a, y4a), h4a)
.strafeToLinearHeading(new Vector2d(x4b, y4b), h4b,
new TranslationalVelConstraint(pickup1Speed));
shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot);
TELE.addData("Red?", redAlliance); TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault); TELE.addData("Turret Default", turrDefault);
TELE.update(); TELE.update();
} }
@@ -925,11 +809,9 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
assert shoot0 != null;
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot0.build(),
manageFlywheel( manageFlywheel(
shoot0Vel, shoot0Vel,
shoot0Hood, shoot0Hood,
@@ -947,169 +829,19 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
); );
Actions.runBlocking( robot.frontLeft.setPower(-0.4);
new ParallelAction( robot.frontRight.setPower(-0.4);
pickup1.build(), robot.backLeft.setPower(-0.4);
manageFlywheel( robot.backRight.setPower(-0.4);
shootAllVelocity,
shootAllHood,
intake1Time,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(intake1Time),
detectObelisk(
intake1Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
obeliskTurrPos1
)
) sleep (sleepTime);
);
motif = turret.getObeliskID(); robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
if (motif == 0) motif = 22;
Actions.runBlocking(
new ParallelAction(
manageFlywheel(
shootAllVelocity,
shootAllHood,
shoot1Time,
0.501,
0.501,
0.501,
0.501
),
shoot1.build(),
prepareShootAll(colorSenseTime, shoot1Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
manageShooterAuto(
intake2Time,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(intake2Time),
detectObelisk(
intake2Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
obeliskTurrPos2
)
)
);
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shoot2Time,
0.501,
0.501,
0.501,
0.501
),
shoot2.build(),
prepareShootAll(colorSenseTime, shoot2Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
);
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
manageShooterAuto(
intake3Time,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(intake3Time),
detectObelisk(
intake3Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
obeliskTurrPos3
)
)
);
Actions.runBlocking(
new ParallelAction(
manageFlywheelAuto(
shoot3Time,
0.501,
0.501,
0.501,
0.501
),
shoot3.build(),
prepareShootAll(colorSenseTime, shoot3Time, motif)
)
);
Actions.runBlocking(
new ParallelAction(
manageShooterAuto(
finalShootAllTime,
0.501,
0.501,
0.501,
0.501
),
shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease)
)
);
drive.updatePoseEstimate(); drive.updatePoseEstimate();

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -52,4 +52,7 @@ public class Poses {
public static Pose2d teleStart = new Pose2d(0, 0, 0); public static Pose2d teleStart = new Pose2d(0, 0, 0);
public static Pose2d teleEnd = new Pose2d(0, 0, 0);
} }

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.teleop; package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
@@ -289,6 +290,12 @@ public class TeleopV3 extends LinearOpMode {
} }
} }
if (gamepad2.square) {
teleEnd = drive.localizer.getPose();
gamepad2.rumble(1000);
}
//EXTRA STUFFINESS: //EXTRA STUFFINESS:
drive.updatePoseEstimate(); drive.updatePoseEstimate();