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 hood0MoveTime = 2;
public static double spindexerSpeedIncrease = 0.014; 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 intake1Time = 3.3;
public static double intake2Time = 3.8; public static double intake2Time = 3.8;
public static double intake3Time = 4.2; public static double intake3Time = 4.2;
public static double flywheel0Time = 1.5; public static double flywheel0Time = 2.2;
public static double pickup1Speed = 12; public static double pickup1Speed = 22;
// ---- 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 = 2.5; public static double shoot1Time = 3.0;
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;
@@ -544,25 +544,17 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot0.build(), shoot0.build(),
autoActions.manageShooterAuto( autoActions.prepareShootAll(
colorSenseTime,
flywheel0Time, flywheel0Time,
motif,
x1, x1,
y1, y1,
h1, h1
false
),
autoActions.detectObelisk(
flywheel0Time,
0.501,
0.501,
0.501,
0.501,
obeliskTurrPosAutoStart
) )
) )
); );
motif = turret.getObeliskID();
} }
@@ -573,15 +565,18 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking( Actions.runBlocking(
new ParallelAction( new ParallelAction(
shoot0.build(), shoot0.build(),
autoActions.manageShooterAuto( autoActions.prepareShootAll(
colorSenseTime,
flywheel0Time, flywheel0Time,
motif,
xShoot0, xShoot0,
yShoot0, yShoot0,
hShoot0, hShoot0
false
) )
) )
); );
} }
void cycleStackClose() { 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.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d; 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.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;
@@ -145,18 +147,35 @@ public class AutoActions {
servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle);
spindexer.detectBalls(true, true);
if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { // Rear Center (Position 1)
driverSlotGreen++; 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)) { // Front Driver (Position 2)
passengerSlotGreen++; 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)) { // Front Passenger (Position 3)
rearSlotGreen++; 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); spindexer.setIntakePower(-0.1);
@@ -178,29 +197,45 @@ public class AutoActions {
if (motif_id == 21) { if (motif_id == 21) {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
spin1PosFirst(); firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = 0.95;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
spin2PosFirst(); firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = 0.05;
} else { } else {
spin3PosFirst(); firstSpindexShootPos = spindexer_outtakeBall3b;
} shootForward = true;
spinEndPos = 0.95; }
} else if (motif_id == 22) { } else if (motif_id == 22) {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
spin2PosFirst(); firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
spin3PosFirst(); firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = 0.95;
} else { } else {
reverseSpin2PosFirst(); firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = false;
spinEndPos = 0.03;
} }
} else { } else {
if (mostGreenSlot == 1) { if (mostGreenSlot == 1) {
spin3PosFirst(); firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = 0.05;
} else if (mostGreenSlot == 2) { } else if (mostGreenSlot == 2) {
oddSpin3PosFirst(); firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = 0.95;
} else { } else {
spin1PosFirst(); firstSpindexShootPos = spindexer_outtakeBall1;
} shootForward = true;
spinEndPos = 0.95; }
} }
return true; return true;