From 475fc4fe1c7c79ad28cede07a18f659b7ee6e5a3 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Mon, 5 Jan 2026 14:57:42 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/teleop/TeleopV2.java | 29 +++++++++---------- .../ftc/teamcode/tests/IntakeTest.java | 2 -- .../utils/PositionalServoProgrammer.java | 25 +++++++++++----- 3 files changed, 31 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index c7673fb..56267d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -320,7 +320,7 @@ public class TeleopV2 extends LinearOpMode { offset -= 360; } - double pos = turrDefault + (error/8); + double pos = turrDefault + (error/8); // adds the overall error to the default TELE.addData("offset", offset); @@ -332,30 +332,29 @@ public class TeleopV2 extends LinearOpMode { pos = 0.97; } - if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ + if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ //not moving AprilTagDetection d20 = aprilTagWebcam.getTagById(20); AprilTagDetection d24 = aprilTagWebcam.getTagById(24); double bearing = 0.0; - if (d20 != null) { + if (d20 != null || d24 != null){ + if (d20 != null) { + bearing = d20.ftcPose.bearing; + } + if (d24 != null) { + bearing = d24.ftcPose.bearing; + } overrideTurr = true; - bearing = d20.ftcPose.bearing; turretPos = servo.getTurrPos() - (bearing/1300); TELE.addData("Bear", bearing); + if (camTicker < 8){ + error += bearing/1300; //adds error in first 8 frames of seeing the tag to see how much to adjust the default pos + } + camTicker++; } - if (d24 != null) { - overrideTurr = true; - bearing = d24.ftcPose.bearing; - turretPos = servo.getTurrPos() - (bearing/1300); - TELE.addData("Bear", bearing); - } - if (camTicker < 8){ - error += bearing/1300; - } - camTicker++; - }else{ + } else { camTicker = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 07ae970..1271584 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -89,8 +89,6 @@ public class IntakeTest extends LinearOpMode { } else if (servo.spinEqual(spindexer_intakePos3)){ spindexerPos = spindexer_intakePos2; } - } else { - spindexerPos = spindexer_outtakeBall1; } } 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 098e0a8..b951b5f 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 @@ -16,9 +16,14 @@ public class PositionalServoProgrammer extends LinearOpMode { Servos servo; public static double spindexPos = 0.501; + public static double spindexPow = 0.0; + public static double spinHoldPow = 0.0; public static double turretPos = 0.501; + public static double turretPow = 0.0; + public static double turrHoldPow = 0.0; public static double transferPos = 0.501; public static double hoodPos = 0.501; + public static int mode = 0; //0 for positional, 1 for power @Override public void runOpMode() throws InterruptedException { @@ -28,21 +33,27 @@ public class PositionalServoProgrammer extends LinearOpMode { waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ - if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ + if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){ double pos = servo.setSpinPos(spindexPos); robot.spin1.setPower(pos); robot.spin2.setPower(-pos); - } else{ - robot.spin1.setPower(0); - robot.spin2.setPower(0); + } else if (mode == 0){ + robot.spin1.setPower(spinHoldPow); + robot.spin2.setPower(spinHoldPow); + } else { + robot.spin1.setPower(spindexPow); + robot.spin2.setPower(-spindexPow); } if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ double pos = servo.setTurrPos(turretPos); robot.turr1.setPower(pos); robot.turr2.setPower(-pos); + } else if (mode == 0){ + robot.turr1.setPower(turrHoldPow); + robot.turr2.setPower(turrHoldPow); } else { - robot.turr1.setPower(0); - robot.turr2.setPower(0); + robot.spin1.setPower(turretPow); + robot.spin2.setPower(-turretPow); } if (transferPos != 0.501){ robot.transferServo.setPosition(transferPos); @@ -51,8 +62,6 @@ public class PositionalServoProgrammer extends LinearOpMode { robot.hood.setPosition(hoodPos); } TELE.addData("spindexer", servo.getSpinPos()); - TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3)); - TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3)); TELE.addData("turret", servo.getTurrPos()); TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); TELE.addData("hood voltage", robot.hoodPos.getVoltage());