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

sorting ahh thing

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-26 22:18:36 -06:00
parent 194100e3c8
commit dc9886855b
8 changed files with 226 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,10 +5,16 @@ import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos0;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
@@ -127,6 +133,7 @@ public class Auto_LT_Close extends LinearOpMode {
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
double waitToPickupGate = 0;
double obeliskTurrPosAutoStart = 0;
// initialize path variables here
TrajectoryActionBuilder shoot0 = null;
@@ -168,19 +175,45 @@ public class Auto_LT_Close extends LinearOpMode {
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
servos.setSpinPos(spinStartPos);
servos.setSpinPos(spindexer_intakePos1);
servos.setTransferPos(transferServo_out);
limelightUsed = true;
limelightUsed = false;
robot.light.setPosition(1);
hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU();
while (opModeInInit()) {
if (limelightUsed && !gateCycle){
Actions.runBlocking(
autoActions.detectObelisk(
0.1,
0.501,
0.501,
0.501,
0.501,
obeliskTurrPosAutoStart
)
);
motif = turret.getObeliskID();
if (motif == 21){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall1;
} else if (motif == 22){
AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b;
} else {
AutoActions.firstSpindexShootPos = spindexer_outtakeBall2;
}
}
if (!gateCycle) {
turret.pipelineSwitch(1);
} else if (redAlliance) {
turret.pipelineSwitch(4);
} else {
turret.pipelineSwitch(2);
}
if (gateCycle) {
servos.setHoodPos(hoodGate0Start);
@@ -188,8 +221,6 @@ public class Auto_LT_Close extends LinearOpMode {
servos.setHoodPos(shoot0Hood);
}
turret.setTurret(turrDefault);
if (gamepad2.crossWasPressed()) {
redAlliance = !redAlliance;
}
@@ -209,24 +240,17 @@ public class Auto_LT_Close extends LinearOpMode {
ballCycles--;
}
if (gamepad2.triangleWasPressed()){
gateCycle = !gateCycle;
}
if (gamepad2.squareWasPressed()) {
drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0));
if (!gateCycle) {
turret.pipelineSwitch(1);
} else if (redAlliance) {
turret.pipelineSwitch(4);
} else {
turret.pipelineSwitch(2);
}
robot.limelight.start();
limelightUsed = true;
gamepad2.rumble(500);
}
@@ -283,6 +307,7 @@ public class Auto_LT_Close extends LinearOpMode {
pickupGateBY = rPickupGateBY;
pickupGateBH = rPickupGateBH;
obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
obeliskTurrPos1 = turrDefault + redObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + redObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + redObeliskTurrPos3;
@@ -338,6 +363,7 @@ public class Auto_LT_Close extends LinearOpMode {
pickupGateBY = bPickupGateBY;
pickupGateBH = bPickupGateBH;
obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;
@@ -414,10 +440,6 @@ public class Auto_LT_Close extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
}
pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot)))
.strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a))
.strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b),
@@ -432,13 +454,13 @@ public class Auto_LT_Close extends LinearOpMode {
waitToPickupGate = waitToPickupGateSolo;
}
teleStart = drive.localizer.getPose();
TELE.addData("Red?", redAlliance);
TELE.addData("Turret Default", turrDefault);
TELE.addData("Gate Cycle?", gateCycle);
TELE.addData("Ball Cycles", ballCycles);
TELE.addData("Limelight Started?", limelightUsed);
TELE.addData("Motif", motif);
TELE.update();
}
@@ -528,9 +550,19 @@ public class Auto_LT_Close extends LinearOpMode {
y1,
h1,
false
),
autoActions.detectObelisk(
flywheel0Time,
0.501,
0.501,
0.501,
0.501,
obeliskTurrPosAutoStart
)
)
);
motif = turret.getObeliskID();
}
@@ -561,24 +593,10 @@ public class Auto_LT_Close extends LinearOpMode {
x2b,
y2b,
h2b
),
autoActions.detectObelisk(
intake1Time,
x2b,
y2b,
posXTolerance,
posYTolerance,
obeliskTurrPos1
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = 22;
prevMotif = motif;
double posX;
double posY;
double posH;
@@ -616,24 +634,10 @@ public class Auto_LT_Close extends LinearOpMode {
x3b,
y3b,
h3b
),
autoActions.detectObelisk(
intake2Time,
x3b,
y3b,
posXTolerance,
posYTolerance,
obeliskTurrPos2
)
)
);
motif = turret.getObeliskID();
if (motif == 0) motif = prevMotif;
prevMotif = motif;
double posX;
double posY;
double posH;
@@ -670,16 +674,7 @@ public class Auto_LT_Close extends LinearOpMode {
x4b,
y4b,
h4b
),
autoActions.detectObelisk(
intake3Time,
x4b,
y4b,
posXTolerance,
posYTolerance,
obeliskTurrPos3
)
)
);

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

@@ -48,7 +48,7 @@ public class AutoActions {
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double firstSpindexShootPos = spinStartPos;
public static double firstSpindexShootPos = spinStartPos;
private boolean shootForward = true;
public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos;
@@ -85,13 +85,13 @@ public class AutoActions {
void spin1PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall1;
shootForward = true;
spinEndPos = spindexer_outtakeBall3 + 0.1;
spinEndPos = 0.95;
}
void spin2PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall2;
shootForward = false;
spinEndPos = spindexer_outtakeBall3b - 0.1;
spinEndPos = 0.05;
}
void reverseSpin2PosFirst() {
@@ -103,13 +103,13 @@ public class AutoActions {
void spin3PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall3;
shootForward = false;
spinEndPos = spindexer_outtakeBall1 - 0.1;
spinEndPos = 0.05;
}
void oddSpin3PosFirst() {
firstSpindexShootPos = spindexer_outtakeBall3b;
shootForward = true;
spinEndPos = spindexer_outtakeBall2 + 0.1;
spinEndPos = 0.95;
}
Action manageShooter = null;
@@ -119,6 +119,9 @@ public class AutoActions {
if (ticker == 0) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(time, posX, posY, posH, false);
driverSlotGreen = 0;
passengerSlotGreen = 0;
rearSlotGreen = 0;
}
ticker++;
servos.setTransferPos(transferServo_out);
@@ -130,6 +133,12 @@ public class AutoActions {
manageShooter.run(telemetryPacket);
TELE.addData("Most Green Slot", mostGreenSlot);
TELE.addData("Driver Slot Greeness", driverSlotGreen);
TELE.addData("Passenger Slot Greeness", passengerSlotGreen);
TELE.addData("Rear Greeness", rearSlotGreen);
TELE.update();
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
@@ -150,7 +159,7 @@ public class AutoActions {
rearSlotGreen++;
}
spindexer.setIntakePower(1);
spindexer.setIntakePower(-0.1);
decideGreenSlot = true;
@@ -242,9 +251,9 @@ public class AutoActions {
boolean end;
if (shootForward) {
end = prevSpinPos > spinEndPos;
end = servos.getSpinPos() > spinEndPos;
} else {
end = prevSpinPos < spinEndPos;
end = servos.getSpinPos() < spinEndPos;
}
if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) {
@@ -380,7 +389,6 @@ public class AutoActions {
manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) {
spindexer.setIntakePower(-0.1);
return false;
} else {
return true;
@@ -408,6 +416,7 @@ public class AutoActions {
double stamp = 0.0;
int ticker = 0;
int prevMotif = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -418,18 +427,23 @@ public class AutoActions {
if (ticker == 0) {
stamp = System.currentTimeMillis();
turret.pipelineSwitch(1);
ticker++;
}
ticker++;
motif = turret.detectObelisk();
if (prevMotif == motif){
ticker++;
}
prevMotif = motif;
turret.setTurret(turrPos);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance;
boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull() || ticker > 10;
teleStart = currentPose;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -10,10 +10,10 @@ public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 155;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double bx2a = 41, by2a = -18, bh2a = -140;
public static double rx2b = 21, ry2b = 34, rh2b = 155.1;
public static double rx2b = 21, ry2b = 34, rh2b = 140.1;
public static double bx2b = 23, by2b = -36, bh2b = -140.1;
public static double rx3a = 55, ry3a = 39, rh3a = 140;

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

@@ -33,12 +33,14 @@ public class ServoPositions {
public static double turret_blueClose = 0;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14;
public static double blueObeliskTurrPos1 = -0.12;
public static double blueObeliskTurrPos2 = -0.13;
public static double blueObeliskTurrPos3 = -0.14;
public static double redObeliskTurrPos0 = -0.35;
public static double redObeliskTurrPos1 = 0.15;
public static double redObeliskTurrPos2 = 0.16;
public static double redObeliskTurrPos3 = 0.17;
public static double blueObeliskTurrPos0 = 0.35;
public static double blueObeliskTurrPos1 = -0.15;
public static double blueObeliskTurrPos2 = -0.16;
public static double blueObeliskTurrPos3 = -0.17;
public static double redTurretShootPos = 0.05;
public static double blueTurretShootPos = -0.05;

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java
View File

@@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.utils.Robot;
@@ -28,16 +29,20 @@ public class ColorTest extends LinearOpMode {
if (isStopRequested()) return;
while(opModeIsActive()){
double green1 = robot.color1.getNormalizedColors().green;
double blue1 = robot.color1.getNormalizedColors().blue;
double red1 = robot.color1.getNormalizedColors().red;
NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors();
double gP1 = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue);
double dist1 = robot.color1.getDistance(DistanceUnit.MM);
color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance);
TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
TELE.addData("Color1 green", gP1);
TELE.addData("Color1 distance (mm)", color1Distance);
// ----- COLOR 2 -----
double green2 = robot.color2.getNormalizedColors().green;
double blue2 = robot.color2.getNormalizedColors().blue;

107
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java Normal file
View File

@@ -0,0 +1,107 @@
package org.firstinspires.ftc.teamcode.tests;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Light;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
@Config
@TeleOp
public class SortingTest extends LinearOpMode {
Robot robot;
MultipleTelemetry TELE;
MecanumDrive drive;
Servos servos;
Spindexer spindexer;
Flywheel flywheel;
Turret turret;
Targeting targeting;
Targeting.Settings targetingSettings;
AutoActions autoActions;
Light light;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
flywheel = new Flywheel(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0, 0.0);
spindexer = new Spindexer(hardwareMap);
servos = new Servos(hardwareMap);
turret = new Turret(robot, TELE, robot.limelight);
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
light = Light.getInstance();
light.init(robot.light, spindexer, turret);
autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light);
int motif = 21;
boolean intaking = true;
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
spindexer.setIntakePower(1);
robot.transfer.setPower(1);
if (gamepad1.crossWasPressed()){
motif = 21;
} else if (gamepad1.squareWasPressed()){
motif = 22;
} else if (gamepad1.triangleWasPressed()){
motif = 23;
}
flywheel.manageFlywheel(2500);
if (gamepad1.leftBumperWasPressed()){
intaking = false;
Actions.runBlocking(
autoActions.prepareShootAll(
3,
5,
motif,
0.501,
0.501,
0.501
)
);
} else if (gamepad1.rightBumperWasPressed()){
intaking = false;
Actions.runBlocking(
autoActions.shootAllAuto(
3.5,
0.014
)
);
intaking = true;
} else if (intaking){
spindexer.processIntake();
}
}
}
}

20
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 < 48) {
if (distanceRearCenter < 52) {
// Mark Ball Found
newPos1Detection = true;
@@ -200,9 +200,9 @@ public class Spindexer {
// FIXIT - Add filtering to improve accuracy.
if (gP >= 0.38) {
ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green
ballPositions[0].ballColor = BallColor.GREEN; // green
} else {
ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple
ballPositions[0].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[spindexerBallPos].ballColor = BallColor.GREEN; // green
ballPositions[2].ballColor = BallColor.GREEN; // green
} else {
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
ballPositions[2].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.42) {
ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green
if (gP >= 0.4) {
ballPositions[1].ballColor = BallColor.GREEN; // green
} else {
ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple
ballPositions[1].ballColor = BallColor.PURPLE; // purple
}
}
} else {
@@ -671,8 +671,10 @@ public class Spindexer {
return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor;
}
private double prevPow = 0.501;
private boolean firstIntakePow = true;
public void setIntakePower(double pow){
if (prevPow != 0.501 && prevPow != pow){
if (firstIntakePow || prevPow != pow){
firstIntakePow = false;
robot.intake.setPower(pow);
}
prevPow = pow;

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

@@ -14,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.constants.Color;
import java.util.ArrayList;
import java.util.List;
@Config
@@ -130,7 +131,7 @@ public class Turret {
}
}
if (xPos != null){
if (zPos>0) {
if (zPos<0) {
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
@@ -171,8 +172,12 @@ public class Turret {
LLResult result = webcam.getLatestResult();
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
double prevTx = -1000;
for (LLResultTypes.FiducialResult fiducial : fiducials) {
obeliskID = fiducial.getFiducialId();
double currentTx = fiducial.getTargetXDegrees();
if (currentTx > prevTx){
obeliskID = fiducial.getFiducialId();
}
}
}
return obeliskID;