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/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); } - }