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 941f3da..42bb3bf 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 @@ -16,11 +16,11 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; - public static double spinStartPos = 0.25; + public static double spinStartPos = 0.22; public static double spinEndPos = 0.85; public static double shootAllAutoSpinStartPos = 0.2; - public static double shootAllSpindexerSpeedIncrease = 0.04; + public static double shootAllSpindexerSpeedIncrease = 0.02; public static double shootAllTime = 1.8; public static double transferServo_out = 0.15; 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 17f35f7..8e31068 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 @@ -117,7 +117,7 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { - TELE.addData("Is limelight on?", robot.limelight.getStatus()); + //TELE.addData("Is limelight on?", robot.limelight.getStatus()); // LIGHT COLORS spindexer.ballCounterLight(); @@ -259,21 +259,21 @@ public class TeleopV3 extends LinearOpMode { if (shooterTicker == 0) { spindexer.prepareShootAllContinous(); - TELE.addLine("preparing to shoot"); + //TELE.addLine("preparing to shoot"); // } else if (shooterTicker == 2) { // //robot.transferServo.setPosition(transferServo_in); // spindexer.shootAll(); // TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { robot.transferServo.setPosition(transferServo_in); - TELE.addLine("shoot"); + //TELE.addLine("shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; shootAll = false; spindexer.resetSpindexer(); //spindexer.processIntake(); - TELE.addLine("stop shooting"); + //TELE.addLine("stop shooting"); } shooterTicker++; //spindexer.processIntake(); @@ -311,11 +311,11 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("oddColor", oddBallColor); // // // Spindexer Debug - TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); - TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); - TELE.addData("spinIntakeState", spindexer.currentIntakeState); - TELE.addData("spinTestCounter", spindexer.counter); - TELE.addData("autoSpintake", autoSpintake); +// TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); +// TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); +// TELE.addData("spinIntakeState", spindexer.currentIntakeState); +// TELE.addData("spinTestCounter", spindexer.counter); +// TELE.addData("autoSpintake", autoSpintake); // // TELE.addData("shootall commanded", shootAll); // // Targeting Debug @@ -324,13 +324,13 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData( "robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting Interpolate", turretInterpolate); - 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("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.update(); +// TELE.update(); ticker++; } 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 56b70a9..d2b29a6 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 @@ -49,16 +49,25 @@ public class Targeting { 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][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); + KNOWNTARGETING[1][3] = new Settings (2800.0, 0.68); // Real settings replaced with (0,3) new Settings (2800.0, 0.62); + KNOWNTARGETING[1][4] = new Settings (3000.0, 0.58); // Real setting replaced with (0,4) new Settings (3000.0, 0.55); KNOWNTARGETING[1][5] = new Settings (3200.0, 0.50); // ROW 2 +// 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); 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); + KNOWNTARGETING[2][3] = new Settings (2800.0, 0.62); // Real settings replaced with (1,3) new Settings (2900.0, 0.53); + KNOWNTARGETING[2][4] = new Settings (3000.0, 0.55); // real settings replaces with (1,4) new Settings (3100.0, 0.50); + KNOWNTARGETING[2][5] = new Settings (3200.0, 0.50); // real settings replaced with (1,5) new Settings (3100.0, 0.50); // ROW 3 KNOWNTARGETING[3][0] = new Settings (2900.0, 0.50); KNOWNTARGETING[3][1] = new Settings (2900.0, 0.50); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 8640b30..e02b496 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -54,7 +54,7 @@ public class Turret { LLResult result; private PIDController bearingPID; - public static double B_PID_P = 0.3, B_PID_I = 0.0, B_PID_D = 0.05; + public static double B_PID_P = 0.15, B_PID_I = 0.0, B_PID_D = 0.025; boolean bearingAligned = false; public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { @@ -288,15 +288,15 @@ public class Turret { /* ---------------- TELEMETRY ---------------- */ - TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); - TELE.addData("Target Pos", "%.3f", targetTurretPos); - TELE.addData("Current Pos", "%.3f", currentPos); - TELE.addData("Commanded Pos", "%.3f", turretPos); - TELE.addData("LL Valid", result.isValid()); - TELE.addData("LL getTx", result.getTx()); - TELE.addData("LL Offset", offset); - //TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); - TELE.addData("Learned Offset", "%.2f", offset); +// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); +// TELE.addData("Target Pos", "%.3f", targetTurretPos); +// TELE.addData("Current Pos", "%.3f", currentPos); +// TELE.addData("Commanded Pos", "%.3f", turretPos); +// TELE.addData("LL Valid", result.isValid()); +// TELE.addData("LL getTx", result.getTx()); +// TELE.addData("LL Offset", offset); +// TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); +// TELE.addData("Learned Offset", "%.2f", offset); } }