diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index fce535b..e234fcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -814,6 +814,8 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("hood", robot.hood.getPosition()); TELE.addData("targetVel", vel); TELE.addData("Velocity", flywheel.getVelo()); + TELE.addData("Velo1", flywheel.velo1); + TELE.addData("Velo2", flywheel.velo2); TELE.addData("shootOrder", shootOrder); TELE.addData("oddColor", oddBallColor); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index f188686..39f1a50 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -18,17 +18,16 @@ import org.firstinspires.ftc.teamcode.utils.Spindexer; @Config @TeleOp public class ShooterTest extends LinearOpMode { - public static int mode = 1; public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED public static double Velocity = 0.0; - public static double P = 40.0; - public static double I = 0.3; - public static double D = 7.0; - public static double F = 10.0; + public static double P = 255.0; + public static double I = 0.0; + public static double D = 0.0; + public static double F = 7.5; public static double transferPower = 1.0; public static double hoodPos = 0.501; public static double turretPos = 0.501; @@ -43,6 +42,9 @@ public class ShooterTest extends LinearOpMode { public double spinPow = 0.09; + public static boolean enableHoodAutoOpen = false; + public double hoodAdjust = 0.0; + public static double hoodAdjustFactor = 1.0; Spindexer spindexer ; @Override @@ -73,7 +75,11 @@ public class ShooterTest extends LinearOpMode { } if (hoodPos != 0.501) { - robot.hood.setPosition(hoodPos); + if (enableHoodAutoOpen) { + robot.hood.setPosition(hoodPos+(hoodAdjustFactor*(flywheel.getVelo()/Velocity))); + } else { + robot.hood.setPosition(hoodPos); + } } if (intake) { 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 d3af293..b03db69 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 @@ -9,8 +9,8 @@ public class Flywheel { Robot robot; public PIDFCoefficients shooterPIDF1, shooterPIDF2; double velo = 0.0; - double velo1 = 0.0; - double velo2 = 0.0; + public double velo1 = 0.0; + public double velo2 = 0.0; double targetVelocity = 0.0; double powPID = 0.0; boolean steady = false; @@ -58,10 +58,6 @@ public class Flywheel { targetVelocity = commandedVelocity; // 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.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 347b850..f15d03b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -29,10 +29,10 @@ public class Robot { public DcMotorEx intake; public DcMotorEx transfer; public PIDFCoefficients shooterPIDF; - public double shooterPIDF_P = 10.0; - public double shooterPIDF_I = 0.6; - public double shooterPIDF_D = 5.0; - public double shooterPIDF_F = 10.0; + public double shooterPIDF_P = 255.0; + public double shooterPIDF_I = 0.0; + public double shooterPIDF_D = 0.0; + public double shooterPIDF_F = 7.5; public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; 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 index d9850a5..38ce24c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -9,8 +9,8 @@ import com.qualcomm.robotcore.hardware.HardwareMap; public class Targeting { MultipleTelemetry TELE; - double cancelOffsetX = 7.071067811; - double cancelOffsetY = 7.071067811; + double cancelOffsetX = 0.0; // was -40.0 + double cancelOffsetY = 0.0; // was 7.0 double unitConversionFactor = 0.95; int tileSize = 24; //inches @@ -37,33 +37,33 @@ public class Targeting { static { KNOWNTARGETING = new Settings[6][6]; // ROW 0 - Closet to the goals - KNOWNTARGETING[0][0] = new Settings (3000.0, 0.25); - KNOWNTARGETING[0][1] = new Settings (3001.0, 0.25); - KNOWNTARGETING[0][2] = new Settings (3002.0, 0.25); - KNOWNTARGETING[0][3] = new Settings (3302.0, 0.2); - KNOWNTARGETING[0][4] = new Settings (3503.0, 0.15); - KNOWNTARGETING[0][5] = new Settings (3505.0, 0.15); + KNOWNTARGETING[0][0] = new Settings (2300.0, 0.93); + KNOWNTARGETING[0][1] = new Settings (2300.0, 0.93); + KNOWNTARGETING[0][2] = new Settings (2500.0, 0.78); + KNOWNTARGETING[0][3] = new Settings (2800.0, 0.68); + KNOWNTARGETING[0][4] = new Settings (3000.0, 0.58); + KNOWNTARGETING[0][5] = new Settings (3000.0, 0.58); // ROW 1 - KNOWNTARGETING[1][0] = new Settings (3010.0, 0.25); - KNOWNTARGETING[1][1] = new Settings (3011.0, 0.25); - KNOWNTARGETING[1][2] = new Settings (3012.0, 0.25); - KNOWNTARGETING[1][3] = new Settings (3313.0, 0.15); - KNOWNTARGETING[1][4] = new Settings (3514.0, 0.15); - KNOWNTARGETING[1][5] = new Settings (3515.0, 0.15); + KNOWNTARGETING[1][0] = new Settings (2300.0, 0.93); + KNOWNTARGETING[1][1] = new Settings (2300.0, 0.93); + KNOWNTARGETING[1][2] = new Settings (2600.0, 0.78); + KNOWNTARGETING[1][3] = new Settings (2800.0, 0.62); + KNOWNTARGETING[1][4] = new Settings (3000.0, 0.55); + KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50); // ROW 2 - KNOWNTARGETING[2][0] = new Settings (3020.0, 0.1); - KNOWNTARGETING[2][1] = new Settings (3000.0, 0.25); - KNOWNTARGETING[2][2] = new Settings (3000.0, 0.15); - KNOWNTARGETING[2][3] = new Settings (3000.0, 0.15); - KNOWNTARGETING[2][4] = new Settings (3524.0, 0.15); - KNOWNTARGETING[2][5] = new Settings (3525.0, 0.15); + KNOWNTARGETING[2][0] = new Settings (2500.0, 0.78); + KNOWNTARGETING[2][1] = new Settings (2500.0, 0.78); + KNOWNTARGETING[2][2] = new Settings (2700.0, 0.60); + KNOWNTARGETING[2][3] = new Settings (2900.0, 0.53); + KNOWNTARGETING[2][4] = new Settings (3100.0, 0.50); + KNOWNTARGETING[2][5] = new Settings (3100.0, 0.50); // ROW 3 - KNOWNTARGETING[3][0] = new Settings (3030.0, 0.15); - KNOWNTARGETING[3][1] = new Settings (3031.0, 0.15); - KNOWNTARGETING[3][2] = new Settings (3000.0, 0.15); - KNOWNTARGETING[3][3] = new Settings (3000.0, 0.15); - KNOWNTARGETING[3][4] = new Settings (3000.0, 0.03); - KNOWNTARGETING[3][5] = new Settings (3535.0, 0.1); + KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50); + KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50); + KNOWNTARGETING[3][2] = new Settings (2900.0, 0.50); + KNOWNTARGETING[3][3] = new Settings (3100.0, 0.47); + KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47); + KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); // ROW 4 KNOWNTARGETING[4][0] = new Settings (4540.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 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; + double rotatedY = (robotX + cancelOffsetX) * sin45 + (robotY + cancelOffsetY) * cos45; + double rotatedX = (robotX + cancelOffsetX) * cos45 - (robotY + cancelOffsetY) * sin45; // Convert robot coordinates to inches robotInchesX = rotatedX * unitConversionFactor; @@ -107,8 +107,8 @@ public class Targeting { // basic search if(!interpolate) { if ((robotGridY < 6) && (robotGridX <6)) { - recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; } return recommendedSettings; } else {