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

Commit working auto front

Browse Source
This commit is contained in:
Keshav Anand
2026-02-27 17:22:01 -06:00
parent e92f11bc69
commit 395d4439db
7 changed files with 63 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -59,19 +59,19 @@ public class Auto_LT_Close extends LinearOpMode {
public static double hood0MoveTime = 2; public static double hood0MoveTime = 2;
public static double spindexerSpeedIncrease = 0.014; public static double spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 10.0; public static double shootAllTime = 6.0;
public static double intake1Time = 3.3; public static double intake1Time = 3.3;
public static double intake2Time = 3.8; public static double intake2Time = 4.2;
public static double intake3Time = 4.2; public static double intake3Time = 5.8;
public static double flywheel0Time = 2.2; public static double flywheel0Time = 1.9;
public static double pickup1Speed = 14; public static double pickup1Speed = 14;
// ---- POSITION TOLERANCES ---- // ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0; public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0; public static double posYTolerance = 5.0;
// ---- OBELISK DETECTION ---- // ---- OBELISK DETECTION ----
public static double shoot1Time = 8; public static double shoot1Time = 2.5;
public static double shoot2Time = 2.5; public static double shoot2Time = 2.5;
public static double shoot3Time = 2.5; public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2; public static double colorSenseTime = 1.2;
@@ -478,15 +478,15 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) { if (gateCycle) {
startAutoGate(); startAutoGate();
shoot(); shoot(0.501, 0.501, 0.501);
cycleStackMiddleGate(); cycleStackMiddleGate();
shoot(); shoot(0.501,0.501, 0.501);
while (getRuntime() - stamp < endGateTime) { while (getRuntime() - stamp < endGateTime) {
cycleGateIntake(); cycleGateIntake();
if (getRuntime() - stamp < lastShootTime) { if (getRuntime() - stamp < lastShootTime) {
cycleGateShoot(); cycleGateShoot();
shoot(); shoot(0.501, 0.501, 0.501);
} }
} }
cycleStackCloseIntakeGate(); cycleStackCloseIntakeGate();
@@ -495,25 +495,28 @@ public class Auto_LT_Close extends LinearOpMode {
cycleStackCloseShootGate(); cycleStackCloseShootGate();
} }
shoot(); shoot(0.501, 0.501, 0.501);
} else { } else {
startAuto(); startAuto();
shoot(); shoot(0.501, 0.501,0.501);
if (ballCycles > 0) { if (ballCycles > 0) {
cycleStackClose(); cycleStackClose();
shoot(); shoot(xShoot, yShoot, hShoot);
} }
if (ballCycles > 1) { if (ballCycles > 1) {
cycleStackMiddle(); cycleStackMiddle();
shoot(); shoot(xShoot, yShoot, hShoot);
} }
if (ballCycles > 2) { if (ballCycles > 2) {
cycleStackFar(); cycleStackFar();
shoot(); shoot(xLeave, yLeave, hLeave);
} }
} }
@@ -534,8 +537,8 @@ public class Auto_LT_Close extends LinearOpMode {
} }
void shoot() { void shoot(double x, double y, double z) {
Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)); Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z));
} }
void startAuto() { void startAuto() {
@@ -545,7 +548,7 @@ public class Auto_LT_Close extends LinearOpMode {
new ParallelAction( new ParallelAction(
shoot0.build(), shoot0.build(),
autoActions.prepareShootAll( autoActions.prepareShootAll(
colorSenseTime, 0.8,
flywheel0Time, flywheel0Time,
motif, motif,
x1, x1,

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

@@ -321,7 +321,7 @@ public class Auto_LT_Far extends LinearOpMode {
void shoot(){ void shoot(){
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease) autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501)
) )
); );

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
View File

@@ -21,6 +21,7 @@ import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -207,7 +208,8 @@ public class AutoActions {
} else { } else {
firstSpindexShootPos = spindexer_outtakeBall3b; firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true; shootForward = true;
spinEndPos = 0.95; } spinEndPos = 0.95;
}
} else if (motif_id == 22) { } else if (motif_id == 22) {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
firstSpindexShootPos = spindexer_outtakeBall3b; firstSpindexShootPos = spindexer_outtakeBall3b;
@@ -253,7 +255,7 @@ public class AutoActions {
}; };
} }
public Action shootAllAuto(double shootTime, double spindexSpeed) { public Action shootAllAuto(double shootTime, double spindexSpeed, double posX, double posY, double posH) {
return new Action() { return new Action() {
int ticker = 1; int ticker = 1;
@@ -275,7 +277,7 @@ public class AutoActions {
if (ticker == 1) { if (ticker == 1) {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501, false); manageShooter = manageShooterAuto(shootTime, posX, posY, posH, false);
} }
ticker++; ticker++;
@@ -423,7 +425,8 @@ public class AutoActions {
manageShooter.run(telemetryPacket); manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) { if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
servos.setSpinPos(spindexer_intakePos1);
return false; return false;
} else { } else {
return true; return true;
@@ -513,14 +516,26 @@ public class AutoActions {
final boolean timeFallback = (time != 0.501); final boolean timeFallback = (time != 0.501);
@Override @Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) { public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose(); Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) { if (ticker == 0) {
stamp = System.currentTimeMillis(); stamp = System.currentTimeMillis();
if (redAlliance) {
turret.pipelineSwitch(4);
light.setManualLightColor(Color.LightRed);
} else {
turret.pipelineSwitch(2);
light.setManualLightColor(Color.LightBlue);
}
} }
ticker++; ticker++;
@@ -535,14 +550,15 @@ public class AutoActions {
double dx = robotX - goalX; // delta x from robot to goal double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose; Pose2d deltaPose;
if (posX != 0.501) { if (posX != 0.501) {
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
Turret.limelightUsed = false;
} else { } else {
deltaPose = new Pose2d(dx, dy, robotHeading); deltaPose = new Pose2d(dx, dy, robotHeading);
Turret.limelightUsed = true;
} }
Turret.limelightUsed = true;
// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy);

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -5,18 +5,18 @@ import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class ServoPositions { public class ServoPositions {
public static double spindexer_intakePos1 = 0.22; //0.13; public static double spindexer_intakePos1 = 0.18; //0.13;
public static double spindexer_intakePos2 = 0.41; //0.33;//0.5; public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.60; //0.53;//0.66; public static double spindexer_intakePos3 = 0.56; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.88; //0.65; //0.24; public static double spindexer_outtakeBall3 = 0.84; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.31; //0.65; //0.24; public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6; public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.51; //0.27; //0.4; public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4;
public static double spinStartPos = 0.14; public static double spinStartPos = 0.10;
public static double spinEndPos = 0.95; public static double spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.014; public static double shootAllSpindexerSpeedIncrease = 0.014;

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java
View File

@@ -95,7 +95,10 @@ public class SortingTest extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
autoActions.shootAllAuto( autoActions.shootAllAuto(
3.5, 3.5,
0.014 0.014,
0.501,
0.501,
0.501
) )
); );
intaking = true; intaking = true;

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -187,7 +187,7 @@ public class Spindexer {
distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger);
// Position 1 // Position 1
if (distanceRearCenter < 52) { if (distanceRearCenter < 48) {
// Mark Ball Found // Mark Ball Found
newPos1Detection = true; newPos1Detection = true;
@@ -200,9 +200,9 @@ public class Spindexer {
// FIXIT - Add filtering to improve accuracy. // FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) { if (gP >= 0.38) {
ballPositions[0].ballColor = BallColor.GREEN; // green ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[0].ballColor = BallColor.PURPLE; // purple ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
} }
} }
} }
@@ -218,9 +218,9 @@ public class Spindexer {
double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP >= 0.4) { if (gP >= 0.4) {
ballPositions[2].ballColor = BallColor.GREEN; // green ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[2].ballColor = BallColor.PURPLE; // purple ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
} }
} }
} else { } else {
@@ -244,10 +244,10 @@ public class Spindexer {
double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.4) { if (gP >= 0.42) {
ballPositions[1].ballColor = BallColor.GREEN; // green ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else { } else {
ballPositions[1].ballColor = BallColor.PURPLE; // purple ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
} }
} }
} else { } else {

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

@@ -271,6 +271,7 @@ public class Turret {
turretAngleDeg += permanentOffset; turretAngleDeg += permanentOffset;
limelightRead(); limelightRead();
// Active correction if we see the target // Active correction if we see the target
if (result.isValid() && !lockOffset && limelightUsed) { if (result.isValid() && !lockOffset && limelightUsed) {