diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 4e036fa..789f60e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.tests; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -55,7 +56,7 @@ public class ShooterTest extends LinearOpMode { rightShooter.setPower(parameter); leftShooter.setPower(parameter); } else if (mode == 1) { - flywheel.setPIDF(P,I,D,F); + flywheel.setPIDF(P, I, D, F); flywheel.manageFlywheel((int) Velocity); } @@ -63,7 +64,6 @@ public class ShooterTest extends LinearOpMode { robot.hood.setPosition(hoodPos); } - robot.transfer.setPower(transferPower); if (shoot) { robot.transferServo.setPosition(transferServo_in); 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 2542488..88b0e88 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 @@ -16,6 +16,8 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Autonomous @Config public class TurretTest extends LinearOpMode { + + public static boolean zeroTurr = false; @Override public void runOpMode() throws InterruptedException { @@ -41,6 +43,12 @@ public class TurretTest extends LinearOpMode { webcam.update(); webcam.displayAllTelemetry(); + TELE.addData("tpos", turret.getTurrPos()); + + if(zeroTurr){ + turret.zeroTurretEncoder(); + } + TELE.update(); 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 8ebd1c4..041c2c0 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 @@ -8,6 +8,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + @TeleOp @Config public class PositionalServoProgrammer extends LinearOpMode { @@ -25,11 +27,19 @@ public class PositionalServoProgrammer extends LinearOpMode { public static double hoodPos = 0.501; public static int mode = 0; //0 for positional, 1 for power + Turret turret; + @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); + + AprilTagWebcam cam = new AprilTagWebcam(); + + cam.init(robot, TELE); + + turret = new Turret(robot, TELE,cam ); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()){ @@ -66,12 +76,13 @@ public class PositionalServoProgrammer extends LinearOpMode { //TODO: @KeshavAnandCode do the above please TELE.addData("spindexer pos", servo.getSpinPos()); - TELE.addData("turret pos", servo.getTurrPos()); + TELE.addData("turret pos", robot.turr1.getPosition()); TELE.addData("spindexer voltage 1", robot.spin1Pos.getVoltage()); TELE.addData("spindexer voltage 2", robot.spin2Pos.getVoltage()); TELE.addData("hood pos", robot.hood.getPosition()); TELE.addData("transferServo voltage", robot.transferServoPos.getVoltage()); TELE.addData("spindexer pow", robot.spin1.getPower()); + TELE.addData("tpos ", turret.getTurrPos() ); 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 e4345b1..97e4633 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,10 +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); + shooter1.setVelocity(0); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter2.setVelocity(1400); + shooter2.setVelocity(0); 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 44ec29e..e9c277e 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,6 +5,7 @@ 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.hardware.DcMotor; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @@ -12,13 +13,15 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; public class Turret { public static double turretTolerance = 0.02; - public static double turrPosScalar = 1; + public static double turrPosScalar = 0.00011264432; public static double turret180Range = 0.4; public static double turrDefault = 0.4; 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 mult = 0.0; + private boolean lockOffset = false; public static double deltaAngleThreshold = 0.02; public static double angleMultiplier = 0.0; Robot robot; @@ -29,7 +32,7 @@ public class Turret { private double offset = 0.0; private double bearing = 0.0; - + public static double clampTolerance = 0.03; public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { this.TELE = tele; @@ -38,13 +41,18 @@ public class Turret { } public double getTurrPos() { - return turrPosScalar * (robot.intake.getCurrentPosition()); + return turrPosScalar * (robot.intake.getCurrentPosition()) + turrDefault; } - public void manualSetTurret(double pos){ + public void zeroTurretEncoder() { + robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + public void manualSetTurret(double pos) { robot.turr1.setPosition(pos); - robot.turr2.setPosition(1-pos); + robot.turr2.setPosition(1 - pos); } public boolean turretEqual(double pos) { @@ -89,7 +97,13 @@ public class Turret { return obeliskID; } + public void zeroOffset() { + offset = 0.0; + } + public void lockOffset(boolean lock) { + lockOffset = lock; + } /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading @@ -120,10 +134,7 @@ public class Turret { // 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; @@ -131,8 +142,22 @@ public class Turret { double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); - // Clamp to servo range - turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); + double currentEncoderPos = this.getTurrPos(); + + if (!turretEqual(turretPos)) { + double diff = turretPos - currentEncoderPos; + turretPos = turretPos + diff * mult; + } + + if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) { + // Clamp to servo range + turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); + } else { + if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) { + // Slowly learn turret offset (persistent calibration) + offset -= tagBearingDeg * errorLearningRate; + } + } robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos);