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 b7f5dbd..39e45b8 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,17 +5,17 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.06; //0.13; + public static double spindexer_intakePos1 = 0.13; //0.13; - public static double spindexer_intakePos2 = 0.25; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.32; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.5; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24; - public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24; + public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24; - 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 spindexer_outtakeBall2 = 0.7; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4; public static double spinStartPos = 0.05; public static double spinEndPos = 0.95; @@ -27,7 +27,7 @@ public class ServoPositions { public static double hoodAuto = 0.27; - public static double hoodOffset = 0.05; // offset from 0.93 + public static double hoodOffset = -0.04; // offset from 0.93 public static double turret_redClose = 0; public static double turret_blueClose = 0; 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 542f1a6..04fa836 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 @@ -75,6 +75,7 @@ public class TeleopV3 extends LinearOpMode { int intakeTicker = 0; private boolean shootAll = false; + boolean relocalize = false; @Override public void runOpMode() throws InterruptedException { @@ -111,7 +112,6 @@ public class TeleopV3 extends LinearOpMode { light.setState(StateEnums.LightState.MANUAL); limelightUsed = true; - robot.light.setPosition(1); while (opModeInInit()) { robot.limelight.start(); if (redAlliance) { @@ -122,6 +122,7 @@ public class TeleopV3 extends LinearOpMode { light.setManualLightColor(Color.LightBlue); } + robot.light.setPosition(1); light.update(); } @@ -166,8 +167,8 @@ public class TeleopV3 extends LinearOpMode { double robX = drive.localizer.getPose().position.x; double robY = drive.localizer.getPose().position.y; - double robotX = robX - xOffset; - double robotY = robY - yOffset; + double robotX = robX + xOffset; + double robotY = robY + yOffset; double robotHeading = drive.localizer.getPose().heading.toDouble(); double goalX = -15; @@ -182,7 +183,20 @@ public class TeleopV3 extends LinearOpMode { targetingSettings = targeting.calculateSettings (robotX, robotY, robotHeading, 0.0, turretInterpolate); - turret.trackGoal(deltaPose); + //RELOCALIZATION + + if (gamepad2.squareWasPressed()){ + relocalize = !relocalize; + gamepad2.rumble(500); + } + + if (relocalize){ + turret.relocalize(); + xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; + yOffset = (turret.getLimelightX() * 39.3701) - robY; + } else { + turret.trackGoal(deltaPose); + } //VELOCITY AUTOMATIC if (autoVel) { @@ -351,8 +365,8 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", turret.getLimelightX()); - TELE.addData("Tag Pos Y", turret.getLimelightY()); + TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); + TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); TELE.update(); 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 fb13435..b738fac 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 @@ -87,7 +87,7 @@ public class Targeting { public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); - if (redAlliance){ + if (!redAlliance){ sin54 = Math.sin(Math.toRadians(54)); } else { sin54 = Math.sin(Math.toRadians(-54)); 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 5f0087b..b247708 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 @@ -27,7 +27,7 @@ public class Turret { public static double turrMin = 0; public static double turrMax = 1; public static boolean limelightUsed = true; - + public static double limelightPosOffset = 5; public static double manualOffset = 0.0; // public static double visionCorrectionGain = 0.08; // Single tunable gain @@ -99,8 +99,11 @@ public class Turret { return Math.abs(pos - this.getTurrPos()) < turretTolerance; } + public static double alphaPosConstant = 0.3; private void limelightRead() { // only for tracking purposes, not general reads - + Double xPos = null; + Double yPos = null; + Double zPos = null; result = webcam.getLatestResult(); if (result != null) { if (result.isValid()) { @@ -116,13 +119,19 @@ public class Turret { for (LLResultTypes.FiducialResult fiducial : fiducials) { limelightTagPose = fiducial.getRobotPoseTargetSpace(); if (limelightTagPose != null){ - limelightTagX = limelightTagPose.getPosition().x; - limelightTagY = limelightTagPose.getPosition().y; + xPos = limelightTagPose.getPosition().x; + yPos = limelightTagPose.getPosition().y; + zPos = limelightTagPose.getPosition().z; } } } } + if (xPos != null){ + limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX); + limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY); + limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ); + } } public double getBearing() { @@ -138,12 +147,16 @@ public class Turret { Pose3D limelightTagPose; double limelightTagX = 0.0; double limelightTagY = 0.0; + double limelightTagZ = 0.0; public double getLimelightX() { return limelightTagX; } + public double getLimelightY() {return limelightTagY;} + public double getLimelightZ(){return limelightTagZ;} - public double getLimelightY() { - return limelightTagY; + public void relocalize(){ + setTurret(turrDefault); + limelightRead(); } public int detectObelisk() {