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 spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 10.0;
public static double shootAllTime = 6.0;
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;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0;
// ---- OBELISK DETECTION ----
public static double shoot1Time = 8;
public static double shoot1Time = 2.5;
public static double shoot2Time = 2.5;
public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2;
@@ -478,15 +478,15 @@ public class Auto_LT_Close extends LinearOpMode {
if (gateCycle) {
startAutoGate();
shoot();
shoot(0.501, 0.501, 0.501);
cycleStackMiddleGate();
shoot();
shoot(0.501,0.501, 0.501);
while (getRuntime() - stamp < endGateTime) {
cycleGateIntake();
if (getRuntime() - stamp < lastShootTime) {
cycleGateShoot();
shoot();
shoot(0.501, 0.501, 0.501);
}
}
cycleStackCloseIntakeGate();
@@ -495,25 +495,28 @@ public class Auto_LT_Close extends LinearOpMode {
cycleStackCloseShootGate();
}
shoot();
shoot(0.501, 0.501, 0.501);
} else {
startAuto();
shoot();
shoot(0.501, 0.501,0.501);
if (ballCycles > 0) {
cycleStackClose();
shoot();
shoot(xShoot, yShoot, hShoot);
}
if (ballCycles > 1) {
cycleStackMiddle();
shoot();
shoot(xShoot, yShoot, hShoot);
}
if (ballCycles > 2) {
cycleStackFar();
shoot();
shoot(xLeave, yLeave, hLeave);
}
}
@@ -534,8 +537,8 @@ public class Auto_LT_Close extends LinearOpMode {
}
void shoot() {
Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease));
void shoot(double x, double y, double z) {
Actions.runBlocking(autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z));
}
void startAuto() {
@@ -545,7 +548,7 @@ public class Auto_LT_Close extends LinearOpMode {
new ParallelAction(
shoot0.build(),
autoActions.prepareShootAll(
colorSenseTime,
0.8,
flywheel0Time,
motif,
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(){
Actions.runBlocking(
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 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.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -207,7 +208,8 @@ public class AutoActions {
} else {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95; }
spinEndPos = 0.95;
}
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
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() {
int ticker = 1;
@@ -275,7 +277,7 @@ public class AutoActions {
if (ticker == 1) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501, false);
manageShooter = manageShooterAuto(shootTime, posX, posY, posH, false);
}
ticker++;
@@ -423,7 +425,8 @@ public class AutoActions {
manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
if ((System.currentTimeMillis() - stamp) > (time * 1000)) {
servos.setSpinPos(spindexer_intakePos1);
return false;
} else {
return true;
@@ -513,14 +516,26 @@ public class AutoActions {
final boolean timeFallback = (time != 0.501);
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
drive.updatePoseEstimate();
Pose2d currentPose = drive.localizer.getPose();
if (ticker == 0) {
stamp = System.currentTimeMillis();
if (redAlliance) {
turret.pipelineSwitch(4);
light.setManualLightColor(Color.LightRed);
} else {
turret.pipelineSwitch(2);
light.setManualLightColor(Color.LightBlue);
}
}
ticker++;
@@ -535,14 +550,15 @@ public class AutoActions {
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose;
if (posX != 0.501) {
deltaPose = new Pose2d(posX, posY, Math.toRadians(posH));
Turret.limelightUsed = false;
} else {
deltaPose = new Pose2d(dx, dy, robotHeading);
Turret.limelightUsed = true;
}
Turret.limelightUsed = true;
// 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
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_outtakeBall3b = 0.31; //0.65; //0.24;
public static double spindexer_outtakeBall3 = 0.84; //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_outtakeBall1 = 0.51; //0.27; //0.4;
public static double spinStartPos = 0.14;
public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4;
public static double spinStartPos = 0.10;
public static double spinEndPos = 0.95;
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(
autoActions.shootAllAuto(
3.5,
0.014
0.014,
0.501,
0.501,
0.501
)
);
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);
// Position 1
if (distanceRearCenter < 52) {
if (distanceRearCenter < 48) {
// Mark Ball Found
newPos1Detection = true;
@@ -200,9 +200,9 @@ public class Spindexer {
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) {
ballPositions[0].ballColor = BallColor.GREEN; // green
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green
} 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);
if (gP >= 0.4) {
ballPositions[2].ballColor = BallColor.GREEN; // green
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else {
ballPositions[2].ballColor = BallColor.PURPLE; // purple
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
}
}
} else {
@@ -244,10 +244,10 @@ public class Spindexer {
double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP >= 0.4) {
ballPositions[1].ballColor = BallColor.GREEN; // green
if (gP >= 0.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
} else {
ballPositions[1].ballColor = BallColor.PURPLE; // purple
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
}
}
} else {

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

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