From e1745500cc1768a8785ff159aaf48f4f8283b40d Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Wed, 21 Jan 2026 09:28:21 -0600 Subject: [PATCH] Create new targeting class. Fix Flywheel Error with motor2 velocity and include spindexer pos updates. --- .../teamcode/constants/ServoPositions.java | 6 +- .../ftc/teamcode/utils/Flywheel.java | 2 +- .../ftc/teamcode/utils/Targeting.java | 100 ++++++++++++++++++ 3 files changed, 104 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index b0a886f..e286e03 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.39; + public static double spindexer_intakePos1 = 0.19; - public static double spindexer_intakePos2 = 0.55;//0.5; + public static double spindexer_intakePos2 = 0.35;//0.5; - public static double spindexer_intakePos3 = 0.71;//0.66; + public static double spindexer_intakePos3 = 0.51;//0.66; public static double spindexer_outtakeBall3 = 0.47; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 495b9db..ef854f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -74,7 +74,7 @@ public class Flywheel { // Record Current Velocity velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); - velo2 = TPS_to_RPM(robot.shooter1.getVelocity()); // Possible error: should it be shooter2 not shooter1? + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); velo = Math.max(velo1,velo2); // really should be a running average of the last 5 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java new file mode 100644 index 0000000..14b00f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.teamcode.utils; + +import android.provider.Settings; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Targeting { + MultipleTelemetry TELE; + + double unitConversionFactor = 1.02; + + int tileSize = 24; //inches + + public int robotGridX, robotGridY = 0; + + public static class Settings { + public double flywheelRPM = 0.0; + public double hoodAngle = 0.0; + + public Settings (double flywheelRPM, double hoodAngle) { + this.flywheelRPM = flywheelRPM; + this.hoodAngle = hoodAngle; + } + } + + // Known settings discovered using shooter test. + // Keep the fidelity at 1 floor tile for now but we could also half it if more + // accuracy is needed. + public static final Settings[][] KNOWNTARGETING; + static { + KNOWNTARGETING = new Settings[6][6]; + // ROW 0 - Closet to the goals + KNOWNTARGETING[0][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[0][5] = new Settings (4500.0, 0.1); + // ROW 1 + KNOWNTARGETING[1][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[1][5] = new Settings (4500.0, 0.1); + // ROW 2 + KNOWNTARGETING[2][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[2][5] = new Settings (4500.0, 0.1); + // ROW 3 + KNOWNTARGETING[3][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[3][5] = new Settings (4500.0, 0.1); + // ROW 4 + KNOWNTARGETING[4][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[4][5] = new Settings (4500.0, 0.1); + // ROW 1 + KNOWNTARGETING[5][0] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][1] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][2] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][3] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][4] = new Settings (4500.0, 0.1); + KNOWNTARGETING[5][5] = new Settings (4500.0, 0.1); + } + + public Targeting() + { + } + + public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) { + Settings recommendedSettings = new Settings(0.0, 0.0); + + // Convert robot coordinates to inches + double robotInchesX = robotX * unitConversionFactor; + double robotInchesY = robotY * unitConversionFactor; + + // Find approximate location in the grid + robotGridX = Math.floorDiv((int) robotInchesX, tileSize); + robotGridY = Math.floorDiv((int) robotInchesY, tileSize); + + // Use Grid Location to perform lookup + // Keep it simple for now but may want to interpolate results + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + + return recommendedSettings; + } +}