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

best match of our season by far

Browse Source
This commit is contained in:
Keshav Anand
2026-01-31 16:35:40 -06:00
parent 3bc0f21a63
commit b670d9f026
5 changed files with 34 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java
View File

@@ -56,6 +56,7 @@ import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a;
import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset;
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;
@@ -95,7 +96,7 @@ import java.util.Objects;
@Config
@Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double shoot0Vel = 2300, shoot0Hood = 0.93;
public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset;
public static double autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015;
@@ -132,9 +133,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double pickup1Speed = 23;
// ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
public static double shootAllVelocity = 2500;
public static double shootAllHood = 0.78;
public static double shootAllHood = 0.78 + hoodOffset;
// ---- PICKUP POSITION TOLERANCES ----
public static double pickup1XTolerance = 2.0;
public static double pickup1YTolerance = 2.0;

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

@@ -33,7 +33,7 @@ public class ServoPositions {
public static double hoodAuto = 0.27;
public static double hoodOffset = -0.05;
public static double turret_redClose = 0.42;
public static double turret_blueClose = 0.38;

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

@@ -36,7 +36,6 @@ public class TeleopV3 extends LinearOpMode {
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4;
public static double manualOffset = 0.0;
public static double hoodSpeedOffset = 0.01;
public static double turretSpeedOffset = 0.01;
public double vel = 3000;
@@ -63,7 +62,6 @@ public class TeleopV3 extends LinearOpMode {
double headingOffset = 0.0;
int ticker = 0;
double hoodOffset = 0.0;
boolean autoSpintake = false;
boolean enableSpindexerManager = true;
@@ -196,25 +194,23 @@ public class TeleopV3 extends LinearOpMode {
if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
} else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset);
robot.hood.setPosition(hoodDefaultPos);
}
if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= hoodSpeedOffset;
autoHoodOffset -= hoodSpeedOffset;
gamepad2.rumble(80);
} else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += hoodSpeedOffset;
autoHoodOffset += hoodSpeedOffset;
gamepad2.rumble(80);
}
if (gamepad2.dpadLeftWasPressed()) {
manualOffset -= turretSpeedOffset;
Turret.manualOffset -= turretSpeedOffset;
gamepad2.rumble(80);
} else if (gamepad2.dpadRightWasPressed()) {
manualOffset += turretSpeedOffset;
Turret.manualOffset += turretSpeedOffset;
gamepad2.rumble(80);
}
@@ -230,6 +226,8 @@ public class TeleopV3 extends LinearOpMode {
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
}
if (enableSpindexerManager) {
//if (!shootAll) {
spindexer.processIntake();
@@ -320,20 +318,20 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("spinTestCounter", spindexer.counter);
// TELE.addData("autoSpintake", autoSpintake);
//
// TELE.addData("shootall commanded", shootAll);
// // Targeting Debug
// TELE.addData("robotX", robotX);
// TELE.addData("robotY", robotY);
// TELE.addData("robotInchesX", targeting.robotInchesX);
// TELE.addData( "robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate);
// TELE.addData("Targeting GridX", targeting.robotGridX);
// TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
//
// TELE.update();
TELE.addData("shootall commanded", shootAll);
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update();
ticker++;
}

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

@@ -4,6 +4,8 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
public class Targeting {
// Known settings discovered using shooter test.
// Keep the fidelity at 1 floor tile for now but we could also half it if more
@@ -183,7 +185,7 @@ public class Targeting {
if (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
}
return recommendedSettings;
} else {

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

@@ -1,8 +1,5 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.manualOffset;
import static java.lang.Math.abs;
import com.acmerobotics.dashboard.config.Config;
@@ -24,11 +21,13 @@ public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
public static double turrDefault = 0.39;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static boolean limelightUsed = true;
public static double manualOffset = 0.0;
public static double visionCorrectionGain = 0.08; // Single tunable gain
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
public static double cameraBearingEqual = 0.5; // Deadband
@@ -36,28 +35,23 @@ public class Turret {
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
double tx = 0.0;
double ty = 0.0;
double limelightPosX = 0.0;
double limelightPosY = 0.0;
LLResult result;
boolean bearingAligned = false;
private boolean lockOffset = false;
private int obeliskID = 0;
private double offset = 0.0;
private double currentTrackOffset = 0.0;
private int currentTrackCount = 0;
private double permanentOffset = 0.0;
LLResult result;
private PIDController bearingPID;
public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
boolean bearingAligned = false;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
@@ -149,7 +143,6 @@ public class Turret {
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
private double bearingAlign(LLResult llResult) {
double bearingOffset = 0.0;
double targetTx = llResult.getTx(); // How far left or right the target is (degrees)
@@ -166,8 +159,7 @@ public class Turret {
}
// Only with valid data and if too far off target
if (llResult.isValid() && !bearingAligned)
{
if (llResult.isValid() && !bearingAligned) {
// Adjust Robot Speed based on how far the target is located
// Only drive at half speed max
@@ -189,7 +181,6 @@ public class Turret {
return bearingOffset;
}
public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */