Arkm/Ftc-12926
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

prelunch

Browse Source
This commit is contained in:
Arkm20
2026-03-20 10:42:34 -05:00
parent 41aebd18ab
commit 527b93a9f3
6 changed files with 58 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java
View File

@@ -52,18 +52,6 @@ public class Auton {
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
.build();
grabSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(59, 15), new Pose(41, 60)))
.setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
.addPath(new BezierCurve(new Pose(41, 60), new Pose(17.711, 56.837), new Pose(25.306, 62.677), new Pose(16, 63)))
.setConstantHeadingInterpolation(Math.toRadians(180))
.build();
scoreSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(16, 63), new Pose(59, 15)))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(118))
.build();
grabEndGamePath = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(59, 15), new Pose(37.157, 26.830), new Pose(10.400, 8)))
.setLinearHeadingInterpolation(Math.toRadians(118), Math.toRadians(180))
@@ -81,7 +69,13 @@ public class Auton {
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(new BezierLine(new Pose(10.439, 17.195), new Pose(59, 15)))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135))
.addPath(new BezierLine(new Pose(59, 15), new Pose(54.614, 19.545)))
.addPath(new BezierCurve(new Pose(59, 15), new Pose(35.728, 26.614), new Pose(10, 27)))
.setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(180))
.addPath(new BezierLine(new Pose(10, 27), new Pose(10.219, 17.664)))
.setConstantHeadingInterpolation(Math.toRadians(180))
.addPath(new BezierLine(new Pose(10.219, 17.664), new Pose(59, 15)))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(135))
.addPath(new BezierLine(new Pose(59, 15), new Pose(55, 20)))
.setConstantHeadingInterpolation(Math.toRadians(135))
.build();
}
@@ -110,7 +104,7 @@ public class Auton {
grabSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(41.758, 61.439), new Pose(16, 59.830)))
.setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(180))
.setConstantHeadingInterpolation(Math.toRadians(180))
.build();
scoreSet2Path = follower.pathBuilder()
@@ -119,23 +113,27 @@ public class Auton {
.build();
grabGate1Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(12, 58.3)))
.setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138))
.addPath(new BezierCurve(new Pose(54, 90), new Pose(40.119, 55.595), new Pose(15.300, 64)))
.setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90))
.addPath(new BezierCurve(new Pose(15.300, 64), new Pose(17.445, 52.820), new Pose(7.500, 50)))
.setConstantHeadingInterpolation(Math.toRadians(90))
.build();
scoreGate1Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(12, 58.3), new Pose(41.918, 62.187), new Pose(54, 90)))
.setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144))
.addPath(new BezierCurve(new Pose(7.500, 50), new Pose(41.918, 62.187), new Pose(54, 90)))
.setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144))
.build();
grabGate2Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(12, 58.3)))
.setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(138))
.addPath(new BezierCurve(new Pose(54, 90), new Pose(42.081, 61.914), new Pose(14.600, 63.600)))
.setLinearHeadingInterpolation(Math.toRadians(144), Math.toRadians(90))
.addPath(new BezierCurve(new Pose(14.600, 63.600), new Pose(17.545, 53.278), new Pose(7.500, 50)))
.setConstantHeadingInterpolation(Math.toRadians(90))
.build();
scoreGate2Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(12, 58.3), new Pose(44, 62), new Pose(54, 90)))
.setLinearHeadingInterpolation(Math.toRadians(138), Math.toRadians(144))
.addPath(new BezierCurve(new Pose(7.500, 50), new Pose(44, 62), new Pose(54, 90)))
.setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(144))
.build();
driveToSet3Path = follower.pathBuilder()
@@ -207,18 +205,6 @@ public class Auton {
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
.build();
grabSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(85, 15), new Pose(103, 60)))
.setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
.addPath(new BezierCurve(new Pose(103, 60), new Pose(126.289, 56.837), new Pose(118.694, 62.677), new Pose(128, 63)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.build();
scoreSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(128, 63), new Pose(85, 15)))
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
.build();
grabEndGamePath = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(85, 15), new Pose(106.843, 26.830), new Pose(133.600, 8)))
.setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
@@ -236,7 +222,13 @@ public class Auton {
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Pose(133.561, 17.195), new Pose(85, 15)))
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45))
.addPath(new BezierLine(new Pose(85, 15), new Pose(89.386, 19.545)))
.addPath(new BezierCurve(new Pose(85, 15), new Pose(108.272, 26.614), new Pose(134, 27)))
.setLinearHeadingInterpolation(Math.toRadians(45), Math.toRadians(0))
.addPath(new BezierLine(new Pose(134, 27), new Pose(133.781, 17.664)))
.setConstantHeadingInterpolation(Math.toRadians(0))
.addPath(new BezierLine(new Pose(133.781, 17.664), new Pose(85, 15)))
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(45))
.addPath(new BezierLine(new Pose(85, 15), new Pose(89, 20)))
.setConstantHeadingInterpolation(Math.toRadians(45))
.build();
}
@@ -265,7 +257,7 @@ public class Auton {
grabSet2Path = follower.pathBuilder()
.addPath(new BezierLine(new Pose(102.242, 61.439), new Pose(128, 59.830)))
.setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0))
.setConstantHeadingInterpolation(Math.toRadians(0))
.build();
scoreSet2Path = follower.pathBuilder()
@@ -274,23 +266,27 @@ public class Auton {
.build();
grabGate1Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(132, 58.3)))
.setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42))
.addPath(new BezierCurve(new Pose(90, 90), new Pose(103.881, 55.595), new Pose(128.700, 64)))
.setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90))
.addPath(new BezierCurve(new Pose(128.700, 64), new Pose(126.555, 52.820), new Pose(136.500, 50)))
.setConstantHeadingInterpolation(Math.toRadians(90))
.build();
scoreGate1Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(132, 58.3), new Pose(102.082, 62.187), new Pose(90, 90)))
.setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36))
.addPath(new BezierCurve(new Pose(136.500, 50), new Pose(102.082, 62.187), new Pose(90, 90)))
.setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36))
.build();
grabGate2Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(132, 58.3)))
.setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(42))
.addPath(new BezierCurve(new Pose(90, 90), new Pose(101.919, 61.914), new Pose(129.400, 63.600)))
.setLinearHeadingInterpolation(Math.toRadians(36), Math.toRadians(90))
.addPath(new BezierCurve(new Pose(129.400, 63.600), new Pose(126.455, 53.278), new Pose(136.500, 50)))
.setConstantHeadingInterpolation(Math.toRadians(90))
.build();
scoreGate2Path = follower.pathBuilder()
.addPath(new BezierCurve(new Pose(132, 58.3), new Pose(100, 62), new Pose(90, 90)))
.setLinearHeadingInterpolation(Math.toRadians(42), Math.toRadians(36))
.addPath(new BezierCurve(new Pose(136.500, 50), new Pose(100, 62), new Pose(90, 90)))
.setLinearHeadingInterpolation(Math.toRadians(90), Math.toRadians(36))
.build();
driveToSet3Path = follower.pathBuilder()
@@ -504,35 +500,6 @@ public class Auton {
break;
case SCORE_SET_1:
if (!follower.isBusy()) {
systems.setAutonState(pedroStateMachine.AutonState.SCORING);
if (checkScoringCondition()) {
setPathState(PathState.DRIVE_TO_GRAB_SET_2);
}
}
break;
// --- SET 2 ---
case DRIVE_TO_GRAB_SET_2:
follower.followPath(grabSet2Path, true);
systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
setPathState(PathState.GRAB_SET_2);
break;
case GRAB_SET_2:
if (!follower.isBusy()) {
systems.setAutonState(pedroStateMachine.AutonState.IDLE);
setPathState(PathState.DRIVE_TO_SCORE_SET_2);
}
break;
case DRIVE_TO_SCORE_SET_2:
follower.followPath(scoreSet2Path, true);
systems.initiateTurretSpinUp();
setPathState(PathState.SCORE_SET_2);
break;
case SCORE_SET_2:
if (!follower.isBusy()) {
systems.setAutonState(pedroStateMachine.AutonState.SCORING);
if (checkScoringCondition()) {

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java
View File

@@ -128,7 +128,7 @@ public class DriveTrain {
if (isAnchorActive) {
isAnchorActive = false;
stop();
this.currentState = DriveState.NORMAL;
this.currentState = DriveState.TURBO;
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
View File

@@ -287,6 +287,7 @@ public class Turret {
LLResult result = hardware.getLimelightResult();
if (result != null && result.isValid()) {
offsetTicks = 0;
double tx = result.getTx();
double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
@@ -481,7 +482,7 @@ public class Turret {
}
if (dist > 145.0) {
return 2300.0;
return 2400.0;
}
double x = dist;
@@ -493,7 +494,7 @@ public class Turret {
+ (0.00636447 * x3)
+ (-0.593959 * x2)
+ (26.08276 * x)
+ 1340.12895;
+ 1350.12895;
return Range.clip(velocity, 1300.0, 2450.0);
}
@@ -565,6 +566,9 @@ public class Turret {
distanceOffset = offset;
}
public double getDistanceOffset() {
return distanceOffset;
}
public void setLaunching(boolean state) {
isLaunching = state;

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
View File

@@ -57,7 +57,7 @@ public class Constants {
public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE;
public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER;
public static final double INTAKE_POWER = 1;
public static final double INTAKE_POWER = 0.9;
public static final double EXTAKE_POWER = -0.8;
public static final double LAUNCH_POWER = 1;
public static final double IDLE_POWER = 0;
@@ -113,7 +113,7 @@ public class Constants {
public static final double MAX_POWER = 0.8;
public static final double MAX_INTEGRAL = 0.5;
// PIDF Values P I D F
public static double[] PIDF_NORMAL = {1.3, 0, 2, 13.5};
public static double[] PIDF_NORMAL = {1.3, 0, 2, 16};
public static double[] PIDF_Shooting = {27, 0, 2, 2.4};
@@ -183,7 +183,7 @@ public class Constants {
public static double ARTIFACT_1_WINDOW = 0.35;
public static double ARTIFACT_2_WINDOW = 0.55;
public static double SHOT_TIME_OF_FLIGHT = 0.83;
public static double SHOT_TIME_OF_FLIGHT = 0.43;
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java
View File

@@ -199,7 +199,7 @@ public class StateMachine {
private void handleRobotStateEntry(RobotState newState) {
switch (newState) {
case TELEOP:
m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
m_intake.setIntakeState(Intake.IntakeState.IDLE);
break;
case ESTOP:

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java
View File

@@ -239,11 +239,16 @@ public class finalTeleOp {
if (gamepad2.dpadUpWasPressed()) {
stateMachine.getTurret().updateDistanceOffset(7);
} else if (gamepad2.dpadDownWasPressed()) {
stateMachine.getTurret().updateDistanceOffset(7);
stateMachine.getTurret().updateDistanceOffset(-7);
} else if (gamepad2.dpadLeftWasPressed()) {
stateMachine.getTurret().setDistanceOffset(0);
}
if (gamepad2.dpadRightWasPressed()) {
org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -90;
} else if (gamepad2.dpadLeftWasReleased()) {
org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.ARTIFACT_1 = -150;
}
}
@@ -258,6 +263,7 @@ public class finalTeleOp {
telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
telemetry.addData("Distance offset", stateMachine.getTurret().getDistanceOffset());
telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰");
telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));