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

area code

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-07 19:12:34 -05:00
parent 9ba5aebc8b
commit 08ba099d5b
6 changed files with 129 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -503,7 +503,7 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
startAutoGate();
startAuto();
shoot(0.501, 0.501, 0.501);
cycleStackClose();
shoot(0.501,0.501, 0.501);

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

@@ -79,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode {
public static double shootStackTime = 2;
public static double shootGateTime = 2.5;
public static double colorSenseTime = 1;
public static double intakeStackTime = 2.5;
public static double intakeGateTime = 2;
public static double intakeStackTime = 5;
public static double intakeGateTime = 3.75;
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
@@ -122,6 +122,8 @@ public class Auto_LT_Far extends LinearOpMode {
robot.limelight.pipelineSwitch(1);
turret = new Turret(robot, TELE, robot.limelight);
limelightUsed = false;
// Spindexer.teleop = false;
@@ -190,6 +192,7 @@ public class Auto_LT_Far extends LinearOpMode {
turret.setTurret(turrDefault);
servos.setSpinPos(spinStartPos);
limelightUsed = true;
servos.setTransferPos(transferServo_out);
}
@@ -231,6 +234,7 @@ public class Auto_LT_Far extends LinearOpMode {
gamepad2.rumble(500);
sleep(1000);
turret.setTurret(turrDefault);
limelightUsed = true;
servos.setSpinPos(spinStartPos);
@@ -259,10 +263,10 @@ public class Auto_LT_Far extends LinearOpMode {
shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
limelightUsed = true;
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Limelight On?",limelightUsed);
TELE.addData("Gate Cycle?", gatePickup);
TELE.addData("Pickup Stack?", stack3);
TELE.addData("Start Position", autoStart);
@@ -281,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode {
robot.transfer.setPower(1);
startAuto();
shoot();
if (redAlliance){
shoot(autoStartRX, autoStartRY, autoStartRH);
} else {
shoot(autoStartBX, autoStartBY, autoStartBH);
}
if (stack3){
cycleStackFar();
shoot();
shoot(xShoot, yShoot, hShoot);
}
while (gatePickup && getRuntime() - stamp < endAutoTime){
@@ -294,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode {
break;
}
cycleGatePrepareShoot();
if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
break;
}
shoot();
shoot(xShoot, yShoot, hShoot);
}
if (gatePickup || stack3){
@@ -324,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode {
}
void shoot(){
void shoot(double x, double y, double z){
Actions.runBlocking(
new ParallelAction(
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501)
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z)
)
);
}
void startAuto(){
if (redAlliance){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
flywheel0Time,
0.501,
0.501,
0.501,
autoStartRX,
autoStartRY,
autoStartRH,
true
)
)
);
} else {
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
flywheel0Time,
autoStartBX,
autoStartBY,
autoStartBH,
true
)
)
);
}
}
void leave3Ball(){
@@ -371,9 +396,13 @@ public class Auto_LT_Far extends LinearOpMode {
)
);
servos.setSpinPos(spinStartPos);
spindexer.setIntakePower(-0.1);
Actions.runBlocking(
new ParallelAction(
shoot3.build()
shoot3.build(),
autoActions.manageShooterAuto(
shootStackTime,xShoot, yShoot, hShoot, false
)
)
);
}
@@ -383,7 +412,7 @@ public class Auto_LT_Far extends LinearOpMode {
new ParallelAction(
pickupGate.build(),
autoActions.intake(
intakeStackTime,
intakeGateTime,
pickupGateX,
pickupGateY,
pickupGateH
@@ -393,16 +422,16 @@ public class Auto_LT_Far extends LinearOpMode {
}
void cycleGatePrepareShoot(){
spindexer.setIntakePower(-0.1);
Actions.runBlocking(
new ParallelAction(
shootGate.build(),
autoActions.prepareShootAll(
colorSenseTime,
autoActions.manageShooterAuto(
shootGateTime,
motif,
xShoot,
yShoot,
hShoot
hShoot,
false
)
)
);

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

@@ -10,11 +10,11 @@ public class Back_Poses {
public static double rShootX = 100, rShootY = 60, rShootH = 125.2;
public static double bShootX = 100, bShootY = -60, bShootH = -125.2;
public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145;
public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145;
public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150;
public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150;
public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1;
public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1;
public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1;
public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1;
public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145;
public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145;
@@ -22,8 +22,8 @@ public class Back_Poses {
public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1;
public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1;
public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145;
public static double bPickupGateXA = 60, bPickupGateYA = -83, bPickupGateHA = -145;
public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140;
public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140;
public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145;
public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145;
public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190;

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

@@ -142,6 +142,62 @@ public class TeleopV3 extends LinearOpMode {
while (opModeIsActive()) {
//TELE.addData("Is limelight on?", robot.limelight.getStatus());
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
//}
// RIGHT_BUMPER
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
spindexer.setIntakePower(1);
} else if (gamepad1.cross) {
spindexer.setIntakePower(-1);
} else {
spindexer.setIntakePower(0);
}
// LEFT_BUMPER
if (!shootAll && gamepad1.leftBumperWasReleased()) {
shootStamp = getRuntime();
shootAll = true;
shooterTicker = 0;
}
intakeTicker++;
if (shootAll) {
intakeTicker = 0;
intake = false;
reject = false;
if (shooterTicker == 0) {
spindexer.prepareShootAllContinous();
//TELE.addLine("preparing to shoot");
// } else if (shooterTicker == 2) {
// //servo.setTransferPos(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (spindexer.shootAllComplete()) {
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
//spindexer.processIntake();
//TELE.addLine("stop shooting");
}
shooterTicker++;
//spindexer.processIntake();
}
if (gamepad1.left_stick_button) {
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
}
}
//DRIVETRAIN:
@@ -169,9 +225,9 @@ public class TeleopV3 extends LinearOpMode {
//TURRET TRACKING
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robH = drive.localizer.getPose().heading.toDouble();
double robX = currentPose.position.x;
double robY = currentPose.position.y;
double robH = currentPose.heading.toDouble();
double robotX = robX + xOffset;
double robotY = robY + yOffset;
@@ -278,63 +334,7 @@ public class TeleopV3 extends LinearOpMode {
shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01;
}
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
//}
// RIGHT_BUMPER
if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
spindexer.setIntakePower(1);
} else if (gamepad1.cross) {
spindexer.setIntakePower(-1);
} else {
spindexer.setIntakePower(0);
}
// LEFT_BUMPER
if (!shootAll && gamepad1.leftBumperWasReleased()) {
shootStamp = getRuntime();
shootAll = true;
shooterTicker = 0;
}
intakeTicker++;
if (shootAll) {
intakeTicker = 0;
intake = false;
reject = false;
if (shooterTicker == 0) {
spindexer.prepareShootAllContinous();
//TELE.addLine("preparing to shoot");
// } else if (shooterTicker == 2) {
// //servo.setTransferPos(transferServo_in);
// spindexer.shootAll();
// TELE.addLine("starting to shoot");
} else if (spindexer.shootAllComplete()) {
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
//spindexer.processIntake();
//TELE.addLine("stop shooting");
}
shooterTicker++;
//spindexer.processIntake();
}
if (gamepad1.left_stick_button) {
servo.setTransferPos(transferServo_out);
//spindexPos = spindexer_intakePos1;
shootAll = false;
spindexer.resetSpindexer();
}
}
//EXTRA STUFFINESS:
drive.updatePoseEstimate();
for (LynxModule hub : allHubs) {
hub.clearBulkCache();

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java
View File

@@ -71,6 +71,8 @@ public class Drivetrain {
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
}
Pose2d currentPos = drive.localizer.getPose();
brakePos = currentPos;
if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -78,15 +80,9 @@ public class Drivetrain {
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
drive.updatePoseEstimate();
brakePos = drive.localizer.getPose();
autoDrive = true;
} else if (brake > 0.4) {
drive.updatePoseEstimate();
Pose2d currentPos = drive.localizer.getPose();
TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
.strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);

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

@@ -242,6 +242,7 @@ public class Turret {
return bearingOffset;
}
double targetTurretPos;
public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
@@ -273,7 +274,7 @@ public class Turret {
limelightRead();
// Active correction if we see the target
if (result.isValid() && !lockOffset && limelightUsed) {
if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) {
currentTrackOffset += bearingAlign(result);
currentTrackCount++;
@@ -329,7 +330,7 @@ public class Turret {
/* ---------------- ANGLE → SERVO POSITION ---------------- */
double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to physical servo limits
targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));