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

back gate auto is like 80% done

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-16 18:03:52 -06:00
parent 85989d54b9
commit 0f556a193f
11 changed files with 115 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -33,7 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double spindexerSpeedIncrease = 0.008;
public static double spindexerSpeedIncrease = 0.014;
// These values are ADDED to turrDefault
public static double redObeliskTurrPos1 = 0.12;
@@ -355,15 +355,6 @@ public class Auto_LT_Close extends LinearOpMode {
void shoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
@@ -380,12 +371,8 @@ public class Auto_LT_Close extends LinearOpMode {
flywheel0Time,
x1,
y1,
posXTolerance,
posYTolerance,
h1,
false
h1
)
)
);
}
@@ -394,16 +381,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
pickup1.build(),
autoActions.manageShooterAuto(
autoActions.intake(
intake1Time,
x2b,
y2b,
posXTolerance,
posYTolerance,
h2b,
true
h2b
),
autoActions.intake(intake1Time),
autoActions.detectObelisk(
intake1Time,
x2b,
@@ -436,17 +419,15 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shoot1.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot1Time,
motif,
posX,
posY,
posXTolerance,
posYTolerance,
posH,
false
),
shoot1.build(),
autoActions.prepareShootAll(colorSenseTime, shoot1Time, motif)
posH
)
)
);
}
@@ -455,16 +436,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
pickup2.build(),
autoActions.manageShooterAuto(
autoActions.intake(
intake2Time,
x3b,
y3b,
posXTolerance,
posYTolerance,
h3b,
true
h3b
),
autoActions.intake(intake2Time),
autoActions.detectObelisk(
intake2Time,
x3b,
@@ -497,17 +474,14 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shoot2.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot2Time,
motif,
posX,
posY,
posXTolerance,
posYTolerance,
posH,
false
),
shoot2.build(),
autoActions.prepareShootAll(colorSenseTime, shoot2Time, motif)
posH)
)
);
}
@@ -516,16 +490,12 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
autoActions.manageShooterAuto(
autoActions.intake(
intake3Time,
x4b,
y4b,
posXTolerance,
posYTolerance,
h4b,
true
h4b
),
autoActions.intake(intake3Time),
autoActions.detectObelisk(
intake3Time,
x4b,
@@ -545,17 +515,15 @@ public class Auto_LT_Close extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shoot3Time,
motif,
xLeave,
yLeave,
posXTolerance,
posYTolerance,
hLeave,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shoot3Time, motif)
hLeave
)
)
);
}

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

@@ -60,16 +60,16 @@ public class Auto_LT_Far extends LinearOpMode {
public static int pickupStackSpeed = 12;
public static int pickupGateSpeed = 25;
int prevMotif = 0;
public static double spindexerSpeedIncrease = 0.008;
public static double spindexerSpeedIncrease = 0.014;
public static double shootAllTime = 2;
// ---- POSITION TOLERANCES ----
public static double posXTolerance = 5.0;
public static double posYTolerance = 5.0;
public static double shootStackTime = 2;
public static double shootGateTime = 2;
public static double shootGateTime = 2.5;
public static double colorSenseTime = 1;
public static double intakeStackTime = 3;
public static double intakeGateTime = 2.5;
public static double intakeStackTime = 2.5;
public static double intakeGateTime = 2;
public static double redObeliskTurrPos1 = 0.12;
public static double redObeliskTurrPos2 = 0.13;
public static double redObeliskTurrPos3 = 0.14;
@@ -79,7 +79,7 @@ public class Auto_LT_Far extends LinearOpMode {
double obeliskTurrPos1 = 0.0;
double obeliskTurrPos2 = 0.0;
double obeliskTurrPos3 = 0.0;
public static double endAutoTime = 25;
public static double endAutoTime = 26;
// initialize path variables here
TrajectoryActionBuilder leave3Ball = null;
@@ -276,7 +276,14 @@ public class Auto_LT_Far extends LinearOpMode {
}
while (gatePickup && getRuntime() - stamp < endAutoTime){
cycleGatePickup();
cycleGatePickupBalls();
if (getRuntime() - stamp > endAutoTime){
break;
}
cycleGatePrepareShoot();
if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
break;
}
shoot();
}
@@ -307,15 +314,6 @@ public class Auto_LT_Far extends LinearOpMode {
void shoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootAllTime,
0.501,
0.501,
0.501,
0.501,
0.501,
false
),
autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease)
)
@@ -329,10 +327,7 @@ public class Auto_LT_Far extends LinearOpMode {
flywheel0Time,
0.501,
0.501,
0.501,
0.501,
0.501,
false
0.501
)
)
@@ -353,16 +348,12 @@ public class Auto_LT_Far extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
pickup3.build(),
autoActions.manageShooterAuto(
autoActions.intake(
intakeStackTime,
xStackPickupB,
yStackPickupB,
posXTolerance,
posYTolerance,
hStackPickupB,
true
hStackPickupB
),
autoActions.intake(intakeStackTime),
autoActions.detectObelisk(
intakeStackTime,
xStackPickupB,
@@ -382,35 +373,28 @@ public class Auto_LT_Far extends LinearOpMode {
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shoot3.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootStackTime,
motif,
xShoot,
yShoot,
posXTolerance,
posYTolerance,
hShoot,
false
),
shoot3.build(),
autoActions.prepareShootAll(colorSenseTime, shootStackTime, motif)
hShoot)
)
);
}
void cycleGatePickup(){
void cycleGatePickupBalls(){
Actions.runBlocking(
new ParallelAction(
pickupGate.build(),
autoActions.manageShooterAuto(
intakeGateTime,
autoActions.intake(
intakeStackTime,
pickupGateX,
pickupGateY,
posXTolerance,
posYTolerance,
pickupGateH,
true
pickupGateH
),
autoActions.intake(intakeStackTime),
autoActions.detectObelisk(
intakeGateTime,
pickupGateX,
@@ -426,20 +410,20 @@ public class Auto_LT_Far extends LinearOpMode {
if (motif == 0) motif = prevMotif;
prevMotif = motif;
}
void cycleGatePrepareShoot(){
Actions.runBlocking(
new ParallelAction(
autoActions.manageShooterAuto(
shootGate.build(),
autoActions.prepareShootAll(
colorSenseTime,
shootGateTime,
motif,
xShoot,
yShoot,
posXTolerance,
posYTolerance,
hShoot,
false
),
shootGate.build(),
autoActions.prepareShootAll(colorSenseTime, shootGateTime, motif)
hShoot
)
)
);
}

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

@@ -49,11 +49,12 @@ public class AutoActions{
private int passengerSlotGreen = 0;
private int rearSlotGreen = 0;
private int mostGreenSlot = 0;
private double firstSpindexShootPos = spinStartPos;
private double firstSpindexShootPos = spindexer_outtakeBall1;
private boolean shootForward = true;
public static double firstShootTime = 0.3;
public int motif = 0;
double spinEndPos = ServoPositions.spinEndPos;
int waitFirstBallTicks = 4;
public AutoActions(Robot rob, MecanumDrive dri, MultipleTelemetry tel, Servos ser, Flywheel fly, Spindexer spi, Targeting tar, Targeting.Settings tS, Turret tur, Light lig){
this.robot = rob;
@@ -68,7 +69,14 @@ public class AutoActions{
this.light = lig;
}
public Action prepareShootAll(double colorSenseTime, double time, int motif_id) {
public Action prepareShootAll(
double colorSenseTime,
double time,
int motif_id,
double posX,
double posY,
double posH
) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@@ -107,10 +115,12 @@ public class AutoActions{
spinEndPos = spindexer_outtakeBall2 + 0.1;
}
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(time, posX, posY, posH);
}
ticker++;
servos.setTransferPos(transferServo_out);
@@ -120,6 +130,8 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
manageShooter.run(telemetryPacket);
if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) {
spindexerWiggle *= -1.0;
@@ -199,22 +211,16 @@ public class AutoActions{
};
}
private boolean doneShooting = false;
public Action shootAllAuto(double shootTime, double spindexSpeed) {
return new Action() {
int ticker = 1;
double stamp = 0.0;
double velo = 0.0;
int shooterTicker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
velo = flywheel.getVelo();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
@@ -226,9 +232,13 @@ public class AutoActions{
if (ticker == 1) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(shootTime, 0.501, 0.501, 0.501);
}
ticker++;
manageShooter.run(telemetryPacket);
double prevSpinPos = servos.getSpinCmdPos();
boolean end;
@@ -238,18 +248,18 @@ public class AutoActions{
end = prevSpinPos < spinEndPos;
}
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < 2)) {
if (System.currentTimeMillis() - stamp < shootTime*1000 && (!end || shooterTicker < waitFirstBallTicks+1)) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 3) {
if (!servos.spinEqual(firstSpindexShootPos) && shooterTicker < 1) {
servos.setTransferPos(transferServo_out);
servos.setSpinPos(firstSpindexShootPos);
} else {
servos.setTransferPos(transferServo_in);
shooterTicker++;
if (shootForward) {
if (shootForward && shooterTicker > waitFirstBallTicks) {
servos.setSpinPos(prevSpinPos + spindexSpeed);
} else {
} else if (shooterTicker > waitFirstBallTicks){
servos.setSpinPos(prevSpinPos - spindexSpeed);
}
@@ -262,7 +272,6 @@ public class AutoActions{
spindexer.resetSpindexer();
spindexer.processIntake();
doneShooting = true;
return false;
@@ -271,15 +280,21 @@ public class AutoActions{
};
}
public Action intake(double intakeTime) {
public Action intake(
double time,
double posX,
double posY,
double posH
) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
Action manageShooter = null;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = System.currentTimeMillis();
manageShooter = manageShooterAuto(time, posX, posY, posH);
}
ticker++;
@@ -292,7 +307,9 @@ public class AutoActions{
teleStart = drive.localizer.getPose();
return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
manageShooter.run(telemetryPacket);
return ((System.currentTimeMillis() - stamp) < (time * 1000)) && !spindexer.isFull();
}
};
}
@@ -360,22 +377,15 @@ public class AutoActions{
double time,
double posX,
double posY,
double posXTolerance,
double posYTolerance,
double posH,
boolean whileIntaking
double posH
) {
return new Action() {
double stamp = 0.0;
int ticker = 0;
int shootingTicker = 0;
double shootingStamp = 0;
final boolean timeFallback = (time != 0.501);
final boolean posXFallback = (posX != 0.501);
final boolean posYFallback = (posY != 0.501);
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
@@ -418,31 +428,18 @@ public class AutoActions{
servos.setHoodPos(targetingSettings.hoodAngle);
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(targetingSettings.flywheelRPM);
boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000;
boolean xDone = posXFallback && Math.abs(robotX - posX) < posXTolerance;
boolean yDone = posYFallback && Math.abs(robotY - posY) < posYTolerance;
boolean shouldFinish;
if (whileIntaking) {
shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull();
} else {
shouldFinish = timeDone || (xDone && yDone) || doneShooting;
}
boolean shouldFinish = timeDone || flywheel.getSteady();
teleStart = currentPose;
TELE.addData("Steady?", flywheel.getSteady());
TELE.update();
if (shouldFinish) {
doneShooting = false;
return false;
} else {
return true;
}
return !shouldFinish;
}
};
}

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

@@ -14,10 +14,10 @@ public class Back_Poses {
public static double rStackPickupAX = 75, rStackPickupAY = 53, rStackPickupAH = 140;
public static double bStackPickupAX = 75, bStackPickupAY = -53, bStackPickupAH = -140;
public static double rStackPickupBX = 55, rStackPickupBY = 83, rStackPickupBH = 140.1;
public static double rStackPickupBX = 55, rStackPickupBY = 73, rStackPickupBH = 140.1;
public static double bStackPickupBX = 50, bStackPickupBY = -78, bStackPickupBH = -140.1;
public static double rPickupGateX = 65, rPickupGateY = 80, rPickupGateH = 140;
public static double rPickupGateX = 70, rPickupGateY = 90, rPickupGateH = 140;
public static double bPickupGateX = 60, bPickupGateY = -88, bPickupGateH = -140;
public static double autoStartRX = 103, autoStartRY = 63.5, autoStartRH = 50;

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

@@ -39,8 +39,8 @@ public class Front_Poses {
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 55;
public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -55;
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

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

@@ -16,7 +16,7 @@ public class ServoPositions {
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spinStartPos = 0.22;
public static double spinStartPos = 0.3;
public static double spinEndPos = 0.85;
public static double shootAllSpindexerSpeedIncrease = 0.014;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -210,7 +210,7 @@ public class TeleopV3 extends LinearOpMode {
//SHOOTER:
double voltage = robot.voltage.getVoltage();
flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage);
flywheel.manageFlywheel(vel);
//HOOD:

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

@@ -37,7 +37,7 @@ public class ShooterTest extends LinearOpMode {
public static double P = 255.0;
public static double I = 0.0;
public static double D = 0.0;
public static double F = 90;
public static double F = 75;
public static double transferPower = 1.0;
public static double hoodPos = 0.501;
public static double turretPos = 0.501;

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java
View File

@@ -68,17 +68,15 @@ public class Flywheel {
// Add code here to set PIDF based on desired RPM
//robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
//robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
// Record Current Velocity
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
}
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
velo = Math.max(velo1, velo2);
// really should be a running average of the last 5
steady = (Math.abs(commandedVelocity - velo) < 200.0);
steady = (Math.abs(commandedVelocity - velo) < 50);
return powPID;
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java
View File

@@ -31,10 +31,10 @@ public class Robot {
public DcMotorEx intake;
public DcMotorEx transfer;
public PIDFCoefficients shooterPIDF;
public double shooterPIDF_P = 255.0;
public double shooterPIDF_I = 0.0;
public double shooterPIDF_D = 0.0;
public double shooterPIDF_F = 90;
public static double shooterPIDF_P = 255;
public static double shooterPIDF_I = 0.0;
public static double shooterPIDF_D = 0.0;
public static double shooterPIDF_F = 75;
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1;
public DcMotorEx shooter2;

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

@@ -27,7 +27,7 @@ public class Turret {
public static double turret180Range = 0.4;
public static double turrDefault = 0.37;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static double turrMax = 0.8;
public static boolean limelightUsed = true;
public static double manualOffset = 0.0;
@@ -40,7 +40,7 @@ public class Turret {
public static double clampTolerance = 0.03;
//public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
public static double B_PID_P = 0.066, B_PID_I = 0.0, B_PID_D = 0.007;
public static double B_PID_P = 0.08, B_PID_I = 0.0, B_PID_D = 0.007;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;