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

Add Targeting Class with initial values that still need tuning. Connect Targeting Class to TeleOpV3. Clean up unused code in Flywheel class.

Browse Source
This commit is contained in:
Matt9150
2026-01-22 00:00:17 -06:00
parent 33300876ef
commit daccec4fdd
3 changed files with 97 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -33,6 +33,7 @@ import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
@@ -53,6 +54,8 @@ public class TeleopV3 extends LinearOpMode {
public static boolean manualTurret = true; public static boolean manualTurret = true;
public double vel = 3000; public double vel = 3000;
public boolean autoVel = true; public boolean autoVel = true;
public boolean targetingVel = true;
public boolean targetingHood = true;
public double manualOffset = 0.0; public double manualOffset = 0.0;
public boolean autoHood = true; public boolean autoHood = true;
public boolean green1 = false; public boolean green1 = false;
@@ -72,6 +75,8 @@ public class TeleopV3 extends LinearOpMode {
Flywheel flywheel; Flywheel flywheel;
MecanumDrive drive; MecanumDrive drive;
Spindexer spindexer; Spindexer spindexer;
Targeting targeting;
Targeting.Settings targetingSettings;
double autoHoodOffset = 0.0; double autoHoodOffset = 0.0;
int shooterTicker = 0; int shooterTicker = 0;
@@ -146,6 +151,8 @@ public class TeleopV3 extends LinearOpMode {
flywheel = new Flywheel(hardwareMap); flywheel = new Flywheel(hardwareMap);
drive = new MecanumDrive(hardwareMap, teleStart); drive = new MecanumDrive(hardwareMap, teleStart);
spindexer = new Spindexer(hardwareMap); spindexer = new Spindexer(hardwareMap);
targeting = new Targeting();
targetingSettings = new Targeting.Settings(0.0,0.0);
PIDFController tController = new PIDFController(tp, ti, td, tf); PIDFController tController = new PIDFController(tp, ti, td, tf);
@@ -402,13 +409,14 @@ public class TeleopV3 extends LinearOpMode {
pos = 0.83; pos = 0.83;
} }
//SHOOTER:
flywheel.manageFlywheel(vel); targetingSettings = targeting.calculateSettings
(robotX,robotY,robotHeading,0.0);
//VELOCITY AUTOMATIC //VELOCITY AUTOMATIC
if (targetingVel) {
if (autoVel) { vel = targetingSettings.flywheelRPM;
} else if (autoVel) {
vel = velPrediction(distanceToGoal); vel = velPrediction(distanceToGoal);
} else { } else {
vel = manualVel; vel = manualVel;
@@ -430,6 +438,9 @@ public class TeleopV3 extends LinearOpMode {
manualVel = 3100; manualVel = 3100;
} }
//SHOOTER:
flywheel.manageFlywheel(vel);
//TODO: test the camera teleop code //TODO: test the camera teleop code
TELE.addData("posS2", pos); TELE.addData("posS2", pos);
@@ -480,7 +491,9 @@ public class TeleopV3 extends LinearOpMode {
//HOOD: //HOOD:
if (autoHood) { if (targetingHood) {
robot.hood.setPosition(targetingSettings.hoodAngle);
} else if (autoHood) {
robot.hood.setPosition(0.15 + hoodOffset); robot.hood.setPosition(0.15 + hoodOffset);
} else { } else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset); robot.hood.setPosition(hoodDefaultPos + hoodOffset);
@@ -845,16 +858,27 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("shootOrder", shootOrder); TELE.addData("shootOrder", shootOrder);
TELE.addData("oddColor", oddBallColor); TELE.addData("oddColor", oddBallColor);
// Spindexer Debug
TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1));
TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition);
TELE.addData("spinIntakeState", spindexer.currentIntakeState); TELE.addData("spinIntakeState", spindexer.currentIntakeState);
TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("spinTestCounter", spindexer.counter);
TELE.addData("autoSpintake", autoSpintake); TELE.addData("autoSpintake", autoSpintake);
TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); //TELE.addData("distanceRearCenter", spindexer.distanceRearCenter);
TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); //TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver);
TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); //TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger);
TELE.addData("shootall commanded", shootAll); 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 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.addData("timeSinceStamp", getRuntime() - shootStamp);
TELE.update(); TELE.update();
ticker++; ticker++;

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

@@ -1,22 +1,13 @@
package org.firstinspires.ftc.teamcode.utils; package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.kP;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.maxStep;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.PIDFCoefficients;
public class Flywheel { public class Flywheel {
Robot robot; Robot robot;
MultipleTelemetry TELE; public PIDFCoefficients shooterPIDF1, shooterPIDF2;
double initPos = 0.0;
double stamp = 0.0;
double stamp1 = 0.0;
double ticker = 0.0;
double currentPos = 0.0;
double velo = 0.0; double velo = 0.0;
double velo1 = 0.0; double velo1 = 0.0;
double velo2 = 0.0; double velo2 = 0.0;
@@ -25,6 +16,10 @@ public class Flywheel {
boolean steady = false; boolean steady = false;
public Flywheel (HardwareMap hardwareMap) { public Flywheel (HardwareMap hardwareMap) {
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
shooterPIDF1 = new PIDFCoefficients
(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
shooterPIDF2 = new PIDFCoefficients
(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F);
} }
public double getVelo () { public double getVelo () {
@@ -48,10 +43,6 @@ public class Flywheel {
robot.shooterPIDF.d = d; robot.shooterPIDF.d = d;
robot.shooterPIDF.f = f; robot.shooterPIDF.f = f;
} }
private double getTimeSeconds ()
{
return (double) System.currentTimeMillis()/1000.0;
}
// Convert from RPM to Ticks per Second // Convert from RPM to Ticks per Second
private double RPM_to_TPS (double RPM) { return (RPM*28.0)/60.0;} private double RPM_to_TPS (double RPM) { return (RPM*28.0)/60.0;}
@@ -62,13 +53,14 @@ public class Flywheel {
public double manageFlywheel(double commandedVelocity) { public double manageFlywheel(double commandedVelocity) {
targetVelocity = commandedVelocity; targetVelocity = commandedVelocity;
// Turn PIDF for Target Velocities // Add code here to set PIDF based on desired RPM
//robot.shooterPIDF.p = P; //robot.shooterPIDF.p = P;
//robot.shooterPIDF.i = I; //robot.shooterPIDF.i = I;
//robot.shooterPIDF.d = D; //robot.shooterPIDF.d = D;
//robot.shooterPIDF.f = F; //robot.shooterPIDF.f = F;
robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, robot.shooterPIDF); robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));

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

@@ -9,12 +9,17 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
public class Targeting { public class Targeting {
MultipleTelemetry TELE; MultipleTelemetry TELE;
double unitConversionFactor = 1.02; double cancelOffsetX = 7.071067811;
double cancelOffsetY = 7.071067811;
double unitConversionFactor = 0.95;
int tileSize = 24; //inches int tileSize = 24; //inches
public double robotInchesX, robotInchesY = 0.0;
public int robotGridX, robotGridY = 0; public int robotGridX, robotGridY = 0;
public static class Settings { public static class Settings {
public double flywheelRPM = 0.0; public double flywheelRPM = 0.0;
public double hoodAngle = 0.0; public double hoodAngle = 0.0;
@@ -32,47 +37,47 @@ 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 (4500.0, 0.1); KNOWNTARGETING[0][0] = new Settings (3000.0, 0.25);
KNOWNTARGETING[0][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[0][1] = new Settings (3001.0, 0.25);
KNOWNTARGETING[0][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[0][2] = new Settings (3002.0, 0.25);
KNOWNTARGETING[0][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[0][3] = new Settings (3302.0, 0.2);
KNOWNTARGETING[0][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[0][4] = new Settings (3503.0, 0.15);
KNOWNTARGETING[0][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[0][5] = new Settings (3505.0, 0.15);
// ROW 1 // ROW 1
KNOWNTARGETING[1][0] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][0] = new Settings (3010.0, 0.25);
KNOWNTARGETING[1][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][1] = new Settings (3011.0, 0.25);
KNOWNTARGETING[1][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][2] = new Settings (3012.0, 0.25);
KNOWNTARGETING[1][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][3] = new Settings (3313.0, 0.15);
KNOWNTARGETING[1][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][4] = new Settings (3514.0, 0.15);
KNOWNTARGETING[1][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[1][5] = new Settings (3515.0, 0.15);
// ROW 2 // ROW 2
KNOWNTARGETING[2][0] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][0] = new Settings (3020.0, 0.1);
KNOWNTARGETING[2][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][1] = new Settings (3000.0, 0.25);
KNOWNTARGETING[2][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][2] = new Settings (3000.0, 0.15);
KNOWNTARGETING[2][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][3] = new Settings (3000.0, 0.15);
KNOWNTARGETING[2][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][4] = new Settings (3524.0, 0.15);
KNOWNTARGETING[2][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[2][5] = new Settings (3525.0, 0.15);
// ROW 3 // ROW 3
KNOWNTARGETING[3][0] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][0] = new Settings (3030.0, 0.15);
KNOWNTARGETING[3][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][1] = new Settings (3031.0, 0.15);
KNOWNTARGETING[3][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][2] = new Settings (3000.0, 0.15);
KNOWNTARGETING[3][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][3] = new Settings (3000.0, 0.15);
KNOWNTARGETING[3][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][4] = new Settings (3000.0, 0.03);
KNOWNTARGETING[3][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[3][5] = new Settings (3535.0, 0.1);
// ROW 4 // ROW 4
KNOWNTARGETING[4][0] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1);
KNOWNTARGETING[4][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1);
KNOWNTARGETING[4][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1);
KNOWNTARGETING[4][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1);
KNOWNTARGETING[4][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1);
KNOWNTARGETING[4][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1);
// ROW 1 // ROW 1
KNOWNTARGETING[5][0] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1);
KNOWNTARGETING[5][1] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1);
KNOWNTARGETING[5][2] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1);
KNOWNTARGETING[5][3] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1);
KNOWNTARGETING[5][4] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1);
KNOWNTARGETING[5][5] = new Settings (4500.0, 0.1); KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1);
} }
public Targeting() public Targeting()
@@ -82,19 +87,25 @@ public class Targeting {
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) {
Settings recommendedSettings = new Settings(0.0, 0.0); Settings recommendedSettings = new Settings(0.0, 0.0);
double cos45 = Math.cos(Math.toRadians(-45));
double sin45 = Math.sin(Math.toRadians(-45));
double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45;
double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45;
// Convert robot coordinates to inches // Convert robot coordinates to inches
double robotInchesX = robotX * unitConversionFactor; robotInchesX = rotatedX * unitConversionFactor;
double robotInchesY = robotY * unitConversionFactor; robotInchesY = rotatedY * unitConversionFactor;
// Find approximate location in the grid // Find approximate location in the grid
robotGridX = Math.floorDiv((int) robotInchesX, tileSize); robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1);
robotGridY = Math.floorDiv((int) robotInchesY, tileSize); robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
// Use Grid Location to perform lookup // Use Grid Location to perform lookup
// Keep it simple for now but may want to interpolate results // Keep it simple for now but may want to interpolate results
if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle;
}
return recommendedSettings; return recommendedSettings;
} }
} }