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

fixed sxonwe color sorting...jusyt have to have a working auto

Browse Source
This commit is contained in:
Keshav Anand
2026-02-26 23:17:16 -06:00
parent dc9886855b
commit 457eaf5feb
2 changed files with 67 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
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 = 3.5;
public static double shootAllTime = 10.0;
public static double intake1Time = 3.3;
public static double intake2Time = 3.8;
public static double intake3Time = 4.2;
public static double flywheel0Time = 1.5;
public static double pickup1Speed = 12;
public static double flywheel0Time = 2.2;
public static double pickup1Speed = 22;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0;
// ---- OBELISK DETECTION ----
public static double shoot1Time = 2.5;
public static double shoot1Time = 3.0;
public static double shoot2Time = 2.5;
public static double shoot3Time = 2.5;
public static double colorSenseTime = 1.2;
@@ -544,25 +544,17 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageShooterAuto(
autoActions.prepareShootAll(
colorSenseTime,
flywheel0Time,
motif,
x1,
y1,
h1,
false
),
autoActions.detectObelisk(
flywheel0Time,
0.501,
0.501,
0.501,
0.501,
obeliskTurrPosAutoStart
h1
)
)
);
motif = turret.getObeliskID();
}
@@ -573,15 +565,18 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
shoot0.build(),
autoActions.manageShooterAuto(
autoActions.prepareShootAll(
colorSenseTime,
flywheel0Time,
motif,
xShoot0,
yShoot0,
hShoot0,
false
hShoot0
)
)
);
}
void cycleStackClose() {

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

@@ -18,7 +18,9 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
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.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
@@ -145,18 +147,35 @@ public class AutoActions {
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) {
driverSlotGreen++;
// Rear Center (Position 1)
double distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM);
if (distanceRearCenter < 52) {
NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double gP1 = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
if (gP1 >= 0.38) {
rearSlotGreen++;
}
}
if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) {
passengerSlotGreen++;
// Front Driver (Position 2)
double distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM);
if (distanceFrontDriver < 50) {
NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors();
double gP2 = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue);
if (gP2 >= 0.4) {
driverSlotGreen++;
}
}
if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) {
rearSlotGreen++;
// Front Passenger (Position 3)
double distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM);
if (distanceFrontPassenger < 29) {
NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors();
double gP3 = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue);
if (gP3 >= 0.4) {
passengerSlotGreen++;
}
}
spindexer.setIntakePower(-0.1);
@@ -178,29 +197,45 @@ public class AutoActions {
if (motif_id == 21) {
if (mostGreenSlot == 1) {
spin1PosFirst();
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = 0.95;
} else if (mostGreenSlot == 2) {
spin2PosFirst();
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = 0.05;
} else {
spin3PosFirst();
}
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95; }
} else if (motif_id == 22) {
if (mostGreenSlot == 1) {
spin2PosFirst();
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95;
} else if (mostGreenSlot == 2) {
spin3PosFirst();
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = 0.95;
} else {
reverseSpin2PosFirst();
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = false;
spinEndPos = 0.03;
}
} else {
if (mostGreenSlot == 1) {
spin3PosFirst();
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = 0.05;
} else if (mostGreenSlot == 2) {
oddSpin3PosFirst();
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95;
} else {
spin1PosFirst();
}
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = 0.95; }
}
return true;