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.ry4b;
import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; 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.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_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; 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_outtakeBall2;
@@ -95,7 +96,7 @@ import java.util.Objects;
@Config @Config
@Autonomous(preselectTeleOp = "TeleopV3") @Autonomous(preselectTeleOp = "TeleopV3")
public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { 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 autoSpinStartPos = 0.2;
public static double shoot0SpinSpeedIncrease = 0.015; public static double shoot0SpinSpeedIncrease = 0.015;
@@ -132,9 +133,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode {
public static double pickup1Speed = 23; public static double pickup1Speed = 23;
// ---- SECOND SHOT / PICKUP ---- // ---- SECOND SHOT / PICKUP ----
public static double shoot1Vel = 2300; public static double shoot1Vel = 2300;
public static double shoot1Hood = 0.93;
public static double shootAllVelocity = 2500; public static double shootAllVelocity = 2500;
public static double shootAllHood = 0.78; public static double shootAllHood = 0.78 + hoodOffset;
// ---- PICKUP POSITION TOLERANCES ---- // ---- PICKUP POSITION TOLERANCES ----
public static double pickup1XTolerance = 2.0; public static double pickup1XTolerance = 2.0;
public static double pickup1YTolerance = 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 hoodAuto = 0.27;
public static double hoodOffset = -0.05;
public static double turret_redClose = 0.42; public static double turret_redClose = 0.42;
public static double turret_blueClose = 0.38; 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 tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static double spinSpeedIncrease = 0.03; public static double spinSpeedIncrease = 0.03;
public static int resetSpinTicks = 4; public static int resetSpinTicks = 4;
public static double manualOffset = 0.0;
public static double hoodSpeedOffset = 0.01; public static double hoodSpeedOffset = 0.01;
public static double turretSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01;
public double vel = 3000; public double vel = 3000;
@@ -63,7 +62,6 @@ public class TeleopV3 extends LinearOpMode {
double headingOffset = 0.0; double headingOffset = 0.0;
int ticker = 0; int ticker = 0;
double hoodOffset = 0.0;
boolean autoSpintake = false; boolean autoSpintake = false;
boolean enableSpindexerManager = true; boolean enableSpindexerManager = true;
@@ -196,25 +194,23 @@ public class TeleopV3 extends LinearOpMode {
if (targetingHood) { if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset); robot.hood.setPosition(targetingSettings.hoodAngle + autoHoodOffset);
} else { } else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset); robot.hood.setPosition(hoodDefaultPos);
} }
if (gamepad2.dpadUpWasPressed()) { if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= hoodSpeedOffset;
autoHoodOffset -= hoodSpeedOffset; autoHoodOffset -= hoodSpeedOffset;
gamepad2.rumble(80); gamepad2.rumble(80);
} else if (gamepad2.dpadDownWasPressed()) { } else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += hoodSpeedOffset;
autoHoodOffset += hoodSpeedOffset; autoHoodOffset += hoodSpeedOffset;
gamepad2.rumble(80); gamepad2.rumble(80);
} }
if (gamepad2.dpadLeftWasPressed()) { if (gamepad2.dpadLeftWasPressed()) {
manualOffset -= turretSpeedOffset; Turret.manualOffset -= turretSpeedOffset;
gamepad2.rumble(80); gamepad2.rumble(80);
} else if (gamepad2.dpadRightWasPressed()) { } else if (gamepad2.dpadRightWasPressed()) {
manualOffset += turretSpeedOffset; Turret.manualOffset += turretSpeedOffset;
gamepad2.rumble(80); gamepad2.rumble(80);
} }
@@ -230,6 +226,8 @@ public class TeleopV3 extends LinearOpMode {
drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
} }
if (enableSpindexerManager) { if (enableSpindexerManager) {
//if (!shootAll) { //if (!shootAll) {
spindexer.processIntake(); spindexer.processIntake();
@@ -320,20 +318,20 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("spinTestCounter", spindexer.counter);
// TELE.addData("autoSpintake", autoSpintake); // TELE.addData("autoSpintake", autoSpintake);
// //
// TELE.addData("shootall commanded", shootAll); TELE.addData("shootall commanded", shootAll);
// // Targeting Debug // Targeting Debug
// TELE.addData("robotX", robotX); TELE.addData("robotX", robotX);
// TELE.addData("robotY", robotY); TELE.addData("robotY", robotY);
// TELE.addData("robotInchesX", targeting.robotInchesX); TELE.addData("robotInchesX", targeting.robotInchesX);
// TELE.addData( "robotInchesY", targeting.robotInchesY); TELE.addData( "robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate); TELE.addData("Targeting Interpolate", turretInterpolate);
// TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridX", targeting.robotGridX);
// TELE.addData("Targeting GridY", targeting.robotGridY); TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
//
// TELE.update(); TELE.update();
ticker++; 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 com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
public class Targeting { public class Targeting {
// Known settings discovered using shooter test. // Known settings discovered using shooter test.
// Keep the fidelity at 1 floor tile for now but we could also half it if more // 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 (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) { if ((robotGridY < 6) && (robotGridX < 6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle + ServoPositions.hoodOffset;
} }
return recommendedSettings; return recommendedSettings;
} else { } 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; 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 static java.lang.Math.abs;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
@@ -24,11 +21,13 @@ public class Turret {
public static double turretTolerance = 0.02; public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432; public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4; 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 turrMin = 0.15;
public static double turrMax = 0.85; public static double turrMax = 0.85;
public static boolean limelightUsed = true; public static boolean limelightUsed = true;
public static double manualOffset = 0.0;
public static double visionCorrectionGain = 0.08; // Single tunable gain public static double visionCorrectionGain = 0.08; // Single tunable gain
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
public static double cameraBearingEqual = 0.5; // Deadband public static double cameraBearingEqual = 0.5; // Deadband
@@ -36,28 +35,23 @@ public class Turret {
// TODO: tune these values for limelight // TODO: tune these values for limelight
public static double clampTolerance = 0.03; 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; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
Limelight3A webcam; Limelight3A webcam;
double tx = 0.0; double tx = 0.0;
double ty = 0.0; double ty = 0.0;
double limelightPosX = 0.0; double limelightPosX = 0.0;
double limelightPosY = 0.0; double limelightPosY = 0.0;
LLResult result;
boolean bearingAligned = false;
private boolean lockOffset = false; private boolean lockOffset = false;
private int obeliskID = 0; private int obeliskID = 0;
private double offset = 0.0; private double offset = 0.0;
private double currentTrackOffset = 0.0; private double currentTrackOffset = 0.0;
private int currentTrackCount = 0; private int currentTrackCount = 0;
private double permanentOffset = 0.0; private double permanentOffset = 0.0;
LLResult result;
private PIDController bearingPID; 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) { public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele; this.TELE = tele;
@@ -149,7 +143,6 @@ public class Turret {
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/ */
private double bearingAlign(LLResult llResult) { private double bearingAlign(LLResult llResult) {
double bearingOffset = 0.0; double bearingOffset = 0.0;
double targetTx = llResult.getTx(); // How far left or right the target is (degrees) 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 // 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 // Adjust Robot Speed based on how far the target is located
// Only drive at half speed max // Only drive at half speed max
@@ -189,7 +181,6 @@ public class Turret {
return bearingOffset; return bearingOffset;
} }
public void trackGoal(Pose2d deltaPos) { public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */ /* ---------------- FIELD → TURRET GEOMETRY ---------------- */