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

Add Targeting values from shootertesting. Tune flywheel with shootertest. Add additional telemetry.

Browse Source
This commit is contained in:
Matt9150
2026-01-26 01:00:03 -06:00
parent d216ce78fc
commit d1434fbaa8
5 changed files with 50 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -814,6 +814,8 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("hood", robot.hood.getPosition()); TELE.addData("hood", robot.hood.getPosition());
TELE.addData("targetVel", vel); TELE.addData("targetVel", vel);
TELE.addData("Velocity", flywheel.getVelo()); TELE.addData("Velocity", flywheel.getVelo());
TELE.addData("Velo1", flywheel.velo1);
TELE.addData("Velo2", flywheel.velo2);
TELE.addData("shootOrder", shootOrder); TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); TELE.addData("oddColor", oddBallColor);

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

@@ -18,17 +18,16 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer;
@Config @Config
@TeleOp @TeleOp
public class ShooterTest extends LinearOpMode { public class ShooterTest extends LinearOpMode {
public static int mode = 1; public static int mode = 1;
public static double parameter = 0.0; public static double parameter = 0.0;
// --- CONSTANTS YOU TUNE --- // --- CONSTANTS YOU TUNE ---
//TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
public static double Velocity = 0.0; public static double Velocity = 0.0;
public static double P = 40.0; public static double P = 255.0;
public static double I = 0.3; public static double I = 0.0;
public static double D = 7.0; public static double D = 0.0;
public static double F = 10.0; public static double F = 7.5;
public static double transferPower = 1.0; public static double transferPower = 1.0;
public static double hoodPos = 0.501; public static double hoodPos = 0.501;
public static double turretPos = 0.501; public static double turretPos = 0.501;
@@ -43,6 +42,9 @@ public class ShooterTest extends LinearOpMode {
public double spinPow = 0.09; public double spinPow = 0.09;
public static boolean enableHoodAutoOpen = false;
public double hoodAdjust = 0.0;
public static double hoodAdjustFactor = 1.0;
Spindexer spindexer ; Spindexer spindexer ;
@Override @Override
@@ -73,8 +75,12 @@ public class ShooterTest extends LinearOpMode {
} }
if (hoodPos != 0.501) { if (hoodPos != 0.501) {
if (enableHoodAutoOpen) {
robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity)));
} else {
robot.hood.setPosition(hoodPos); robot.hood.setPosition(hoodPos);
} }
}
if (intake) { if (intake) {
robot.intake.setPower(1); robot.intake.setPower(1);

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

@@ -9,8 +9,8 @@ public class Flywheel {
Robot robot; Robot robot;
public PIDFCoefficients shooterPIDF1, shooterPIDF2; public PIDFCoefficients shooterPIDF1, shooterPIDF2;
double velo = 0.0; double velo = 0.0;
double velo1 = 0.0; public double velo1 = 0.0;
double velo2 = 0.0; public double velo2 = 0.0;
double targetVelocity = 0.0; double targetVelocity = 0.0;
double powPID = 0.0; double powPID = 0.0;
boolean steady = false; boolean steady = false;
@@ -58,10 +58,6 @@ public class Flywheel {
targetVelocity = commandedVelocity; targetVelocity = commandedVelocity;
// Add code here to set PIDF based on desired RPM // Add code here to set PIDF based on desired RPM
//robot.shooterPIDF.p = P;
//robot.shooterPIDF.i = I;
//robot.shooterPIDF.d = D;
//robot.shooterPIDF.f = F;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);

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

@@ -29,10 +29,10 @@ public class Robot {
public DcMotorEx intake; public DcMotorEx intake;
public DcMotorEx transfer; public DcMotorEx transfer;
public PIDFCoefficients shooterPIDF; public PIDFCoefficients shooterPIDF;
public double shooterPIDF_P = 10.0; public double shooterPIDF_P = 255.0;
public double shooterPIDF_I = 0.6; public double shooterPIDF_I = 0.0;
public double shooterPIDF_D = 5.0; public double shooterPIDF_D = 0.0;
public double shooterPIDF_F = 10.0; public double shooterPIDF_F = 7.5;
public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
public DcMotorEx shooter1; public DcMotorEx shooter1;
public DcMotorEx shooter2; public DcMotorEx shooter2;

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

@@ -9,8 +9,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
public class Targeting { public class Targeting {
MultipleTelemetry TELE; MultipleTelemetry TELE;
double cancelOffsetX = 7.071067811; double cancelOffsetX = 0.0; // was -40.0
double cancelOffsetY = 7.071067811; double cancelOffsetY = 0.0; // was 7.0
double unitConversionFactor = 0.95; double unitConversionFactor = 0.95;
int tileSize = 24; //inches int tileSize = 24; //inches
@@ -37,33 +37,33 @@ public class Targeting {
static { static {
KNOWNTARGETING = new Settings[6][6]; KNOWNTARGETING = new Settings[6][6];
// ROW 0 - Closet to the goals // ROW 0 - Closet to the goals
KNOWNTARGETING[0][0] = new Settings (3000.0, 0.25); KNOWNTARGETING[0][0] = new Settings (2300.0, 0.93);
KNOWNTARGETING[0][1] = new Settings (3001.0, 0.25); KNOWNTARGETING[0][1] = new Settings (2300.0, 0.93);
KNOWNTARGETING[0][2] = new Settings (3002.0, 0.25); KNOWNTARGETING[0][2] = new Settings (2500.0, 0.78);
KNOWNTARGETING[0][3] = new Settings (3302.0, 0.2); KNOWNTARGETING[0][3] = new Settings (2800.0, 0.68);
KNOWNTARGETING[0][4] = new Settings (3503.0, 0.15); KNOWNTARGETING[0][4] = new Settings (3000.0, 0.58);
KNOWNTARGETING[0][5] = new Settings (3505.0, 0.15); KNOWNTARGETING[0][5] = new Settings (3000.0, 0.58);
// ROW 1 // ROW 1
KNOWNTARGETING[1][0] = new Settings (3010.0, 0.25); KNOWNTARGETING[1][0] = new Settings (2300.0, 0.93);
KNOWNTARGETING[1][1] = new Settings (3011.0, 0.25); KNOWNTARGETING[1][1] = new Settings (2300.0, 0.93);
KNOWNTARGETING[1][2] = new Settings (3012.0, 0.25); KNOWNTARGETING[1][2] = new Settings (2600.0, 0.78);
KNOWNTARGETING[1][3] = new Settings (3313.0, 0.15); KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62);
KNOWNTARGETING[1][4] = new Settings (3514.0, 0.15); KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55);
KNOWNTARGETING[1][5] = new Settings (3515.0, 0.15); KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50);
// ROW 2 // ROW 2
KNOWNTARGETING[2][0] = new Settings (3020.0, 0.1); KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78);
KNOWNTARGETING[2][1] = new Settings (3000.0, 0.25); KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78);
KNOWNTARGETING[2][2] = new Settings (3000.0, 0.15); KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60);
KNOWNTARGETING[2][3] = new Settings (3000.0, 0.15); KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53);
KNOWNTARGETING[2][4] = new Settings (3524.0, 0.15); KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50);
KNOWNTARGETING[2][5] = new Settings (3525.0, 0.15); KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50);
// ROW 3 // ROW 3
KNOWNTARGETING[3][0] = new Settings (3030.0, 0.15); KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][1] = new Settings (3031.0, 0.15); KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][2] = new Settings (3000.0, 0.15); KNOWNTARGETING[3][2] = new Settings (2900.0, 0.50);
KNOWNTARGETING[3][3] = new Settings (3000.0, 0.15); KNOWNTARGETING[3][3] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][4] = new Settings (3000.0, 0.03); KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47);
KNOWNTARGETING[3][5] = new Settings (3535.0, 0.1); KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47);
// ROW 4 // ROW 4
KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
@@ -89,8 +89,8 @@ public class Targeting {
double cos45 = Math.cos(Math.toRadians(-45)); double cos45 = Math.cos(Math.toRadians(-45));
double sin45 = Math.sin(Math.toRadians(-45)); double sin45 = Math.sin(Math.toRadians(-45));
double rotatedY = (robotX - 40.0) * sin45 + (robotY + 7.0) * cos45; double rotatedY = (robotX + cancelOffsetX) * sin45 + (robotY + cancelOffsetY) * cos45;
double rotatedX = (robotX - 40.0) * cos45 - (robotY + 7.0) * sin45; double rotatedX = (robotX + cancelOffsetX) * cos45 - (robotY + cancelOffsetY) * sin45;
// Convert robot coordinates to inches // Convert robot coordinates to inches
robotInchesX = rotatedX * unitConversionFactor; robotInchesX = rotatedX * unitConversionFactor;
@@ -107,8 +107,8 @@ public class Targeting {
// basic search // basic search
if(!interpolate) { if(!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) { if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle;
} }
return recommendedSettings; return recommendedSettings;
} else { } else {