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

Compare commits

base: KeshavAnandCode:b235f9787b25ce6200fa3cfa726df6a484ba1b1d
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:e1615d7647a82881b14dc990ebd6e50e861f9bf6
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
b235f9787b ... e1615d7647

Author SHA1 Message Date
DanTheMan-byte
e1615d7647 League Tournament Success 2026-02-02 18:07:26 -06:00
DanTheMan-byte
b60d64b98f jytrv 2026-01-31 18:15:10 -06:00
4 changed files with 1097 additions and 317 deletions
Expand all files Collapse all files

389
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;
@@ -95,7 +42,7 @@ import java.util.Objects;
@Config @Config
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Far_Indexed extends LinearOpMode { public class Auto_LT_Far_Indexed extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset;
public static double autoSpinStartPos = 0.2; public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0SpinSpeedIncrease = 0.015;
@@ -116,8 +63,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
public static double normalIntakeTime = 3.3; public static double normalIntakeTime = 3.3;
public static double shoot1Turr = 0.57; public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0; public static double shoot0XTolerance = 1.0;
public static double redTurretShootPos = turrDefault + 0.12; public static double redTurretShootPos = turrDefault ;
public static double blueTurretShootPos = turrDefault - 0.14; public static double blueTurretShootPos = turrDefault ;
double turretShootPos = 0.0; double turretShootPos = 0.0;
public static double finalShootAllTime = 3.0; public static double finalShootAllTime = 3.0;
@@ -149,6 +96,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
public static double colorSenseTime = 1.2; public static double colorSenseTime = 1.2;
public static double firstShootTime = 0.3; public static double firstShootTime = 0.3;
public static int fwdTime = 200, strafeTime = 2300;
public int motif = 0; public int motif = 0;
Robot robot; Robot robot;
@@ -186,6 +135,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 +267,8 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
}; };
} }
public static int sleepTime = 1300;
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;
@@ -754,7 +713,7 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
turret.manualSetTurret(turrDefault); turret.manualSetTurret(turrDefault);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); drive = new MecanumDrive(hardwareMap, teleEnd);
robot.spin1.setPosition(autoSpinStartPos); robot.spin1.setPosition(autoSpinStartPos);
robot.spin2.setPosition(1 - autoSpinStartPos); robot.spin2.setPosition(1 - autoSpinStartPos);
@@ -762,20 +721,17 @@ 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);
while (opModeInInit()) { while (opModeInInit()) {
robot.hood.setPosition(shoot0Hood); robot.hood.setPosition(shoot0Hood);
turret.manualSetTurret(turrDefault); turret.manualSetTurret(turretShootPos);
if (gamepad2.crossWasPressed()) { if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance; redAlliance = !redAlliance;
@@ -797,123 +753,52 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
blueObeliskTurrPos2 = turrDefault - 0.13; blueObeliskTurrPos2 = turrDefault - 0.13;
blueObeliskTurrPos3 = turrDefault - 0.14; blueObeliskTurrPos3 = turrDefault - 0.14;
redTurretShootPos = turrDefault + 0.12; redTurretShootPos = turrDefault + 0.05;
blueTurretShootPos = turrDefault - 0.14;
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,16 +810,14 @@ 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,
flywheel0Time, 9,
x1, 0.501,
0.501, 0.501,
shoot0XTolerance, shoot0XTolerance,
0.501 0.501
@@ -943,182 +826,54 @@ public class Auto_LT_Far_Indexed extends LinearOpMode {
) )
); );
Actions.runBlocking( drive.updatePoseEstimate();
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
); teleStart = drive.localizer.getPose();
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease)
pickup1.build(),
manageFlywheel(
shootAllVelocity,
shootAllHood,
intake1Time,
x2b,
y2b,
pickup1XTolerance,
pickup1YTolerance
),
intake(intake1Time),
detectObelisk(
intake1Time,
0.501,
0.501,
obelisk1XTolerance,
obelisk1YTolerance,
obeliskTurrPos1
)
)
); );
motif = turret.getObeliskID(); robot.frontLeft.setPower(-0.4);
robot.frontRight.setPower(-0.4);
robot.backLeft.setPower(-0.4);
robot.backRight.setPower(-0.4);
sleep (fwdTime);
if (motif == 0) motif = 22; robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
sleep (sleepTime);
Actions.runBlocking( robot.frontLeft.setPower(-0.4);
new ParallelAction( robot.frontRight.setPower(0.4);
manageFlywheel( robot.backLeft.setPower(0.4);
shootAllVelocity, robot.backRight.setPower(-0.4);
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();
teleStart = drive.localizer.getPose(); teleStart = drive.localizer.getPose();
TELE.addLine("finished"); sleep (strafeTime);
TELE.update();
robot.frontLeft.setPower(0);
robot.frontRight.setPower(0);
robot.backLeft.setPower(0);
robot.backRight.setPower(0);
while(opModeIsActive()) {
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
}
sleep(2000);
} }

1013
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_Red2.java Normal file
View File

File diff suppressed because it is too large Load Diff

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);
} }

9
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,14 @@ public class TeleopV3 extends LinearOpMode {
} }
} }
if (gamepad2.square) {
drive.updatePoseEstimate();
teleEnd = drive.localizer.getPose();
gamepad2.rumble(1000);
}
//EXTRA STUFFINESS: //EXTRA STUFFINESS:
drive.updatePoseEstimate(); drive.updatePoseEstimate();