diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java index 8effe66..a18be3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java @@ -710,7 +710,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { bearing = result.getTx(); } } - double turretPos = robot.turr1Pos.getCurrentPosition() - (bearing / 1300); + double turretPos = (bearing / 1300); robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1 - turretPos); } 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 e286e03..14e2371 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 @@ -42,4 +42,8 @@ public class ServoPositions { public static double turret_detectBlueClose = 0.6; public static double turrDefault = 0.4; + public static double turrMin = 0.2; + public static double turrMax = 0.8; + + } 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 5027eb7..6ce65a0 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 @@ -150,14 +150,14 @@ public class TeleopV3 extends LinearOpMode { PIDFController tController = new PIDFController(tp, ti, td, tf); tController.setTolerance(0.001); +// +// if (redAlliance) { +// robot.limelight.pipelineSwitch(3); +// } else { +// robot.limelight.pipelineSwitch(2); +// } - if (redAlliance) { - robot.limelight.pipelineSwitch(3); - } else { - robot.limelight.pipelineSwitch(2); - } - - robot.limelight.start(); +// robot.limelight.start(); waitForStart(); if (isStopRequested()) return; @@ -434,28 +434,28 @@ public class TeleopV3 extends LinearOpMode { 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.limelight.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; - } +// 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; +// } if (!overrideTurr) { turretPos = pos; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java index 52edfd8..303544d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java @@ -52,7 +52,6 @@ public class PIDServoTest extends LinearOpMode { } telemetry.addData("pos", pos); - telemetry.addData("Turret Voltage", robot.turr1Pos.getCurrentPosition()); telemetry.addData("Spindex Voltage", robot.spin1Pos.getVoltage()); telemetry.addData("target", target); telemetry.addData("Mode", mode); 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 new file mode 100644 index 0000000..9c8a1cd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.teamcode.tests; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Turret; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +@Autonomous +@Config +public class TurretTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + + Robot robot = new Robot(hardwareMap); + MultipleTelemetry TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + AprilTagWebcam webcam = new AprilTagWebcam(); + webcam.init(robot, TELE); + + Turret turret = new Turret(robot, TELE, webcam); + waitForStart(); + + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(10, 0,0)); + + while(opModeIsActive()){ + + drive.updatePoseEstimate(); + turret.trackGoal(drive.localizer.getPose()); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 86ad2ee..8ebd1c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -71,7 +71,6 @@ public class PositionalServoProgrammer extends LinearOpMode { TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); TELE.addData("hood pos", robot.hood.getPosition()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); - TELE.addData("turret voltage", robot.turr1Pos.getCurrentPosition()); TELE.addData("spindexer pow", robot.spin1.getPower()); 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 6ff9e6f..a3cb45c 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; @@ -14,10 +15,13 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +@Config public class Robot { //Initialize Public Components + public static boolean usingLimelight = false; + public static boolean usingCamera = true; public DcMotorEx frontLeft; public DcMotorEx frontRight; public DcMotorEx backLeft; @@ -29,8 +33,7 @@ public class Robot { public double shooterPIDF_I = 0.6; public double shooterPIDF_D = 5.0; public double shooterPIDF_F = 10.0; - - public double[] shooterPIDF_StepSizes = {10.0,1.0,0.001, 0.0001}; + public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; public DcMotorEx shooter1; public DcMotorEx shooter2; public Servo hood; @@ -41,7 +44,7 @@ public class Robot { public CRServo spin2; public AnalogInput spin1Pos; public AnalogInput spin2Pos; - public DcMotorEx turr1Pos; + public AnalogInput turr1Pos; public AnalogInput transferServoPos; public AprilTagProcessor aprilTagProcessor; public WebcamName webcam; @@ -50,10 +53,6 @@ public class Robot { public RevColorSensorV3 color3; public Limelight3A limelight; - public static boolean usingLimelight = true; - - public static boolean usingCamera = true; - public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -77,7 +76,7 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode shooter1.setDirection(DcMotorSimple.Direction.REVERSE); - shooterPIDF = new PIDFCoefficients(shooterPIDF_P,shooterPIDF_I , shooterPIDF_D, shooterPIDF_F); + 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); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -89,7 +88,7 @@ public class Robot { turr2 = hardwareMap.get(Servo.class, "t2"); - turr1Pos = intake; // Encoder of turret plugged in intake port + turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(CRServo.class, "spin1"); @@ -118,9 +117,9 @@ public class Robot { color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); - if (usingLimelight){ + if (usingLimelight) { limelight = hardwareMap.get(Limelight3A.class, "limelight"); - } else if (usingCamera){ + } else if (usingCamera) { webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 2b7019a..f145927 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -44,17 +44,15 @@ public class Servos { } public double getTurrPos() { - return (double) ((double) robot.turr1Pos.getCurrentPosition() / 1024.0) * ((double) 44.0 / (double) 77.0); + return 1.0; } public double setTurrPos(double pos) { - turretPID.setPIDF(turrP, turrI, turrD, turrF); - - return spinPID.calculate(this.getTurrPos(), pos); + return 1.0; } public boolean turretEqual(double pos) { - return Math.abs(pos - this.getTurrPos()) < 0.01; + return true; } } 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 new file mode 100644 index 0000000..a32b58e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -0,0 +1,142 @@ +package org.firstinspires.ftc.teamcode.utils; + +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 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 turrMin = 0.2; + public static double turrMax = 0.8; + Robot robot; + MultipleTelemetry TELE; + AprilTagWebcam webcam; + private int obeliskID = 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.TELE = tele; + this.robot = rob; + this.webcam = cam; + } + + public double getTurrPos() { + return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3); + + } + + public void manualSetTurret(double pos){ + robot.turr1.setPosition(pos); + robot.turr2.setPosition(1-pos); + } + + public boolean turretEqual(double pos) { + return Math.abs(pos - this.getTurrPos()) < 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 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; + } + return obeliskID; + } + + public int getObeliskID() { + 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( + Math.atan2(deltaPos.position.y, deltaPos.position.x) + ); + + // Robot heading (field → robot) + double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + + // Turret angle needed relative to robot + double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; + + // 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 += offset; + + /* ---------------- ANGLE → SERVO ---------------- */ + + double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0)); + + // Clamp to servo range + turretPos = Math.max(turrMin, Math.min(turretPos, 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); + } + +}