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

Create new targeting class. Fix Flywheel Error with motor2 velocity and include spindexer pos updates.

Browse Source
This commit is contained in:
Matt9150
2026-01-21 09:28:21 -06:00
parent 51bf55cc49
commit e1745500cc
3 changed files with 104 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class ServoPositions { 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; public static double spindexer_outtakeBall3 = 0.47;

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

@@ -74,7 +74,7 @@ public class Flywheel {
// Record Current Velocity // Record Current Velocity
velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); 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); velo = Math.max(velo1,velo2);
// really should be a running average of the last 5 // really should be a running average of the last 5

100
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java Normal file
View File

@@ -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;
}
}