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 1830322..ec6012e 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 @@ -50,6 +50,8 @@ public class TeleopV3 extends LinearOpMode { public static double spindexPos = spindexer_intakePos1; public static double spinPow = 0.09; public static double bMult = 1, bDiv = 2200; + public static double limelightKp = 0.001; // Proportional gain for limelight auto-aim + public static double limelightDeadband = 0.5; // Ignore tx values smaller than this public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; public static boolean manualTurret = true; public double vel = 3000; @@ -121,6 +123,7 @@ public class TeleopV3 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; + boolean turretInterpolate = true; public static double velPrediction(double distance) { if (distance < 30) { @@ -411,7 +414,7 @@ public class TeleopV3 extends LinearOpMode { targetingSettings = targeting.calculateSettings - (robotX,robotY,robotHeading,0.0); + (robotX,robotY,robotHeading,0.0, turretInterpolate); //VELOCITY AUTOMATIC if (targetingVel) { @@ -443,46 +446,100 @@ public class TeleopV3 extends LinearOpMode { //TODO: test the camera teleop code + // TODO: TEST THIS CODE + TELE.addData("posS2", pos); -// if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving -// double bearing; -// -// LLResult result = robot.light.getLatestResult(); -// if (result != null) { -// if (result.isValid()) { -// bearing = result.getTx() * bMult; -// -// double bearingCorrection = bearing / bDiv; -// -// error += bearingCorrection; -// -// camTicker++; -// TELE.addData("tx", bearingCorrection); -// TELE.addData("ty", result.getTy()); -// } -// } -// -// } else { -// camTicker = 0; -// overrideTurr = false; -// } + LLResult result = robot.limelight.getLatestResult(); + boolean limelightActive = false; + + double turretMin = 0.13; + double turretMax = 0.83; + + if (result != null && result.isValid()) { + double tx = result.getTx(); + double ty = result.getTy(); + + if (Math.abs(tx) > limelightDeadband) { + limelightActive = true; + overrideTurr = true; + + double currentTurretPos = servo.getTurrPos(); + + // + tx means tag is right, so rotate right + double adjustment = -tx * limelightKp; + + // calculate new position + double newTurretPos = currentTurretPos + adjustment; + + if (newTurretPos < turretMin) { + double forwardDist = turretMin - newTurretPos; + double backwardDist = (currentTurretPos - turretMin) + (turretMax - newTurretPos); + // check path distance + if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) { + newTurretPos = turretMax - (turretMin - newTurretPos); + } else { + newTurretPos = turretMin; + } + } else if (newTurretPos > turretMax) { + double forwardDist = newTurretPos - turretMax; + double backwardDist = (turretMax - currentTurretPos) + (newTurretPos - turretMin); + if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) { + newTurretPos = turretMin + (newTurretPos - turretMax); + } else { + newTurretPos = turretMax; + } + } + + // Final clamp + if (newTurretPos < turretMin) { + newTurretPos = turretMin; + } else if (newTurretPos > turretMax) { + newTurretPos = turretMax; + } + + pos = newTurretPos; + turretPos = pos; + + camTicker++; + TELE.addData("tx", tx); + TELE.addData("ty", ty); + TELE.addData("limelightAdjustment", adjustment); + TELE.addData("limelightActive", true); + } else { + limelightActive = true; + overrideTurr = true; + TELE.addData("tx", tx); + TELE.addData("ty", ty); + TELE.addData("limelightActive", true); + TELE.addData("limelightStatus", "Centered"); + } + } else { + if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { + TELE.addData("limelightActive", false); + TELE.addData("limelightStatus", "No target"); + } else { + camTicker = 0; + overrideTurr = false; + limelightActive = false; + } + } - if (!overrideTurr) { + if (!limelightActive && !overrideTurr) { turretPos = pos; } TELE.addData("posS3", turretPos); - if (manualTurret) { + if (manualTurret && !limelightActive) { pos = turrDefault + (manualOffset / 100) + error; } - if (!overrideTurr) { + if (!overrideTurr && !limelightActive) { turretPos = pos; } - if (Math.abs(gamepad2.left_stick_x)>0.2) { + if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) { manualOffset += 1.35 * gamepad2.left_stick_x; } @@ -873,6 +930,7 @@ public class TeleopV3 extends LinearOpMode { TELE.addData( "robotY", robotY); 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 9c8a1cd..7401aaa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -1,9 +1,12 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -35,7 +38,29 @@ public class TurretTest extends LinearOpMode { while(opModeIsActive()){ drive.updatePoseEstimate(); - turret.trackGoal(drive.localizer.getPose()); + Pose2d robotPose = drive.localizer.getPose(); + + double dx = goalPose.position.x - robotPose.position.x; + double dy = goalPose.position.y - robotPose.position.y; + + double heading = robotPose.heading.toDouble(); + + // field vector -> robot frame... avoids double calculation + double relX = dx * Math.cos(-heading) - dy * Math.sin(-heading); + double relY = dx * Math.sin(-heading) + dy * Math.cos(-heading); + + Pose2d deltaPos = new Pose2d( + new Vector2d(relX, relY), + robotPose.heading + ); + + + turret.trackGoal(deltaPos); + + TELE.addData("Robot Pose", robotPose); + TELE.addData("Goal Pose", goalPose); + TELE.addData("Delta Pos", deltaPos); + 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 7a2acb0..d802ba2 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 @@ -84,28 +84,58 @@ 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, boolean interpolate) { 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; + 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 robotInchesX = rotatedX * unitConversionFactor; robotInchesY = rotatedY * unitConversionFactor; // Find approximate location in the grid - robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1); + robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); - // Use Grid Location to perform lookup - // Keep it simple for now but may want to interpolate results - if ((robotGridY < 6) && (robotGridX <6)) { - recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + //clamp + robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); + robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); + + // basic search + if(!interpolate) { + if ((robotGridY < 6) && (robotGridX <6)) { + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + } + return recommendedSettings; + } else { + + // bilinear interpolation + int x0 = robotGridX; + int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); + int y0 = gridY; + int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); + + double x = (robotInchesX - (x0 * tileSize)) / tileSize; + double y = (robotInchesY - (y0 * tileSize)) / tileSize; + + double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; + double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; + double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; + double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; + + double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; + double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; + double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; + double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; + + recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; + recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; + + return recommendedSettings; } - return recommendedSettings; } } 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 a32b58e..b73a733 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 @@ -5,79 +5,61 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.util.Range; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Config public class Turret { - public static double turretTolerance = 0.02; - public static double turrPosScalar = 1.009; - public static double turret180Range = 0.6; public static double turrDefault = 0.4; - public static double cameraBearingEqual = 1.5; - public static double errorLearningRate = 2; + public static double turretRange = 0.6; public static double turrMin = 0.2; public static double turrMax = 0.8; - Robot robot; - MultipleTelemetry TELE; - AprilTagWebcam webcam; + + public static double turretTolerance = 0.02; + + public static double cameraBearingEqual = 1.5; + public static double errorLearningRate = 0.02; // must be low + public static double maxOffsetDeg = 30.0; + + private final Robot robot; + private final MultipleTelemetry TELE; + private final AprilTagWebcam webcam; + private int obeliskID = 0; - private double turrPos = 0.0; - private double offset = 0.0; - private double bearing = 0.0; + private double offsetDeg = 0.0; public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { - this.TELE = tele; this.robot = rob; + this.TELE = tele; this.webcam = cam; } - public double getTurrPos() { - return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3); - + public double getTurretPos() { + return robot.turr1Pos.getVoltage() / 3.3; } - public void manualSetTurret(double pos){ + public void manualSetTurret(double pos) { + pos = Range.clip(pos, turrMin, turrMax); robot.turr1.setPosition(pos); - robot.turr2.setPosition(1-pos); + robot.turr2.setPosition(1.0 - pos); } - public boolean turretEqual(double pos) { - return Math.abs(pos - this.getTurrPos()) < turretTolerance; + public boolean turretAt(double pos) { + return Math.abs(pos - getTurretPos()) < turretTolerance; } - public double getBearing() { - if (redAlliance) { - AprilTagDetection d24 = webcam.getTagById(24); - if (d24 != null) { - bearing = d24.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } - } else { - AprilTagDetection d20 = webcam.getTagById(20); - if (d20 != null) { - bearing = d20.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } - } + public double getBearingDeg() { + AprilTagDetection tag = + redAlliance ? webcam.getTagById(24) : webcam.getTagById(20); + return (tag != null) ? tag.ftcPose.bearing : Double.NaN; } public int detectObelisk() { - AprilTagDetection id21 = webcam.getTagById(21); - AprilTagDetection id22 = webcam.getTagById(22); - AprilTagDetection id23 = webcam.getTagById(23); - if (id21 != null) { - obeliskID = 21; - } else if (id22 != null) { - obeliskID = 22; - } else if (id23 != null) { - obeliskID = 23; - } + if (webcam.getTagById(21) != null) obeliskID = 21; + else if (webcam.getTagById(22) != null) obeliskID = 22; + else if (webcam.getTagById(23) != null) obeliskID = 23; return obeliskID; } @@ -85,58 +67,34 @@ public class Turret { return obeliskID; } - public void update() { - - } - - /* - Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading - */ public void trackGoal(Pose2d deltaPos) { - /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ - - // Angle from robot to goal in robot frame - double desiredTurretAngleDeg = Math.toDegrees( + double turretAngleDeg = Math.toDegrees( Math.atan2(deltaPos.position.y, deltaPos.position.x) ); - // Robot heading (field → robot) - double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + double bearingDeg = getBearingDeg(); - // Turret angle needed relative to robot - double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; + if (!Double.isNaN(bearingDeg) && + Math.abs(bearingDeg) < cameraBearingEqual) { - // Normalize to [-180, 180] - while (turretAngleDeg > 180) turretAngleDeg -= 360; - while (turretAngleDeg < -180) turretAngleDeg += 360; - - /* ---------------- APRILTAG CORRECTION ---------------- */ - - double tagBearingDeg = getBearing(); // + = target is to the left - - if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) < cameraBearingEqual) { - // Slowly learn turret offset (persistent calibration) - offset -= tagBearingDeg * errorLearningRate; + offsetDeg -= bearingDeg * errorLearningRate; + offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg); } - turretAngleDeg += offset; + turretAngleDeg += offsetDeg; - /* ---------------- ANGLE → SERVO ---------------- */ + double turretPos = + turrDefault + (turretAngleDeg / 180.0) * turretRange; - double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0)); - - // Clamp to servo range - turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); + turretPos = Range.clip(turretPos, turrMin, turrMax); robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos); - /* ---------------- TELEMETRY ---------------- */ - - TELE.addData("Turret Angle", turretAngleDeg); - TELE.addData("Bearing", tagBearingDeg); - TELE.addData("Offset", offset); + TELE.addData("Turret Angle (deg)", turretAngleDeg); + TELE.addData("Offset (deg)", offsetDeg); + TELE.addData("Tag Bearing (deg)", bearingDeg); + TELE.addData("Turret Servo", turretPos); } - }