diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index d2a1382..bf2fa34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -12,7 +12,7 @@ public class Poses { public static double relativeGoalHeight = goalHeight - turretHeight; - public static Pose2d goalPose = new Pose2d(-15, 0, 0); + public static Pose2d goalPose = new Pose2d(-10, 0, 0); public static double rx1 = 40, ry1 = -7, rh1 = 0; public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); @@ -38,6 +38,6 @@ public class Poses { public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); + public static Pose2d teleStart = new Pose2d(0, 0, 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 ec6012e..bc81e2f 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 @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.teleop; -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; import static org.firstinspires.ftc.teamcode.utils.Servos.spinF; import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; @@ -21,7 +19,6 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.arcrobotics.ftclib.controller.PIDFController; -import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -29,11 +26,13 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.ArrayList; import java.util.List; @@ -50,8 +49,6 @@ 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; @@ -155,7 +152,7 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, teleStart); spindexer = new Spindexer(hardwareMap); targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0,0.0); + targetingSettings = new Targeting.Settings(0.0, 0.0); PIDFController tController = new PIDFController(tp, ti, td, tf); @@ -169,6 +166,12 @@ public class TeleopV3 extends LinearOpMode { // robot.limelight.start(); + AprilTagWebcam webcam = new AprilTagWebcam(); + webcam.init(robot, TELE); + + Turret turret = new Turret(robot, TELE, webcam); + waitForStart(); + waitForStart(); if (isStopRequested()) return; @@ -382,40 +385,22 @@ public class TeleopV3 extends LinearOpMode { double robotY = robY - yOffset; double robotHeading = drive.localizer.getPose().heading.toDouble(); - double goalX = -10; + double goalX = -15; double goalY = 0; - double dx = goalX - robotX; // delta x from robot to goal - double dy = goalY - robotY; // delta y from robot to goal + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; - - desiredTurretAngle += manualOffset + error; - - offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); - - if (offset > 135) { - offset -= 360; - } - - double pos = turrDefault; - - TELE.addData("offset", offset); - - pos -= offset * ((double) 1 / 360); - - if (pos < 0.13) { - pos = 0.13; - } else if (pos > 0.83) { - pos = 0.83; - } - - targetingSettings = targeting.calculateSettings (robotX,robotY,robotHeading,0.0, turretInterpolate); + turret.trackGoal(deltaPose); + + webcam.update(); + //VELOCITY AUTOMATIC if (targetingVel) { vel = targetingSettings.flywheelRPM; @@ -446,105 +431,28 @@ public class TeleopV3 extends LinearOpMode { //TODO: test the camera teleop code - // TODO: TEST THIS CODE - - TELE.addData("posS2", pos); - - 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 (!limelightActive && !overrideTurr) { - turretPos = pos; - } - - TELE.addData("posS3", turretPos); - - if (manualTurret && !limelightActive) { - pos = turrDefault + (manualOffset / 100) + error; - } - - if (!overrideTurr && !limelightActive) { - turretPos = pos; - } - - if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) { - manualOffset += 1.35 * gamepad2.left_stick_x; - } - - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - 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; +// } //HOOD: 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 7401aaa..2542488 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,12 +1,9 @@ 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; @@ -33,33 +30,19 @@ public class TurretTest extends LinearOpMode { Turret turret = new Turret(robot, TELE, webcam); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(10, 0,0)); + + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); while(opModeIsActive()){ drive.updatePoseEstimate(); - Pose2d robotPose = drive.localizer.getPose(); - - double dx = goalPose.position.x - robotPose.position.x; - double dy = goalPose.position.y - robotPose.position.y; + turret.trackGoal(drive.localizer.getPose()); - double heading = robotPose.heading.toDouble(); + webcam.update(); + webcam.displayAllTelemetry(); - // 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/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index a3cb45c..e4345b1 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 @@ -79,8 +79,10 @@ public class Robot { shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter1.setVelocity(1400); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter2.setVelocity(1400); hood = hardwareMap.get(Servo.class, "hood"); 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 b73a733..202293c 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,61 +5,83 @@ 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.4; public static double turrDefault = 0.4; - public static double turretRange = 0.6; + public static double cameraBearingEqual = 1; + public static double errorLearningRate = 0.15; public static double turrMin = 0.2; public static double turrMax = 0.8; - - 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; - + public static double deltaAngleThreshold = 0.02; + public static double angleMultiplier = 0.0; + Robot robot; + MultipleTelemetry TELE; + AprilTagWebcam webcam; private int obeliskID = 0; - private double offsetDeg = 0.0; + private double turrPos = 0.0; + private double offset = 0.0; + private double bearing = 0.0; + + public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { - this.robot = rob; this.TELE = tele; + this.robot = rob; this.webcam = cam; } - public double getTurretPos() { - return robot.turr1Pos.getVoltage() / 3.3; + public double getTurrPos() { + return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3); + } - public void manualSetTurret(double pos) { - pos = Range.clip(pos, turrMin, turrMax); + public void manualSetTurret(double pos){ robot.turr1.setPosition(pos); - robot.turr2.setPosition(1.0 - pos); + robot.turr2.setPosition(1-pos); } - public boolean turretAt(double pos) { - return Math.abs(pos - getTurretPos()) < turretTolerance; + public boolean turretEqual(double pos) { + return Math.abs(pos - this.getTurrPos()) < turretTolerance; } - public double getBearingDeg() { - AprilTagDetection tag = - redAlliance ? webcam.getTagById(24) : webcam.getTagById(20); - return (tag != null) ? tag.ftcPose.bearing : Double.NaN; + 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 int detectObelisk() { - if (webcam.getTagById(21) != null) obeliskID = 21; - else if (webcam.getTagById(22) != null) obeliskID = 22; - else if (webcam.getTagById(23) != null) obeliskID = 23; + 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; + } return obeliskID; } @@ -67,34 +89,59 @@ public class Turret { return obeliskID; } + + + /* + Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading + */ public void trackGoal(Pose2d deltaPos) { - double turretAngleDeg = Math.toDegrees( + /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ + + // Angle from robot to goal in robot frame + double desiredTurretAngleDeg = Math.toDegrees( Math.atan2(deltaPos.position.y, deltaPos.position.x) ); - double bearingDeg = getBearingDeg(); + // Robot heading (field → robot) + double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); - if (!Double.isNaN(bearingDeg) && - Math.abs(bearingDeg) < cameraBearingEqual) { + // Turret angle needed relative to robot + double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; - offsetDeg -= bearingDeg * errorLearningRate; - offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg); + turretAngleDeg = -turretAngleDeg; + + // 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; } - turretAngleDeg += offsetDeg; + turretAngleDeg += offset; - double turretPos = - turrDefault + (turretAngleDeg / 180.0) * turretRange; + /* ---------------- ANGLE → SERVO ---------------- */ - turretPos = Range.clip(turretPos, turrMin, turrMax); + double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + + // Clamp to servo range + turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos); - TELE.addData("Turret Angle (deg)", turretAngleDeg); - TELE.addData("Offset (deg)", offsetDeg); - TELE.addData("Tag Bearing (deg)", bearingDeg); - TELE.addData("Turret Servo", turretPos); + /* ---------------- TELEMETRY ---------------- */ + + TELE.addData("Turret Angle", turretAngleDeg); + TELE.addData("Bearing", tagBearingDeg); + TELE.addData("Offset", offset); } + }