turret

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;
@@ -63,7 +64,6 @@ public class ShooterTest extends LinearOpMode {
                 robot.hood.setPosition(hoodPos);
             }
 
-
             robot.transfer.setPower(transferPower);
             if (shoot) {
                 robot.transferServo.setPosition(transferServo_in);

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();

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

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");
 

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,10 +41,15 @@ public class Turret {
     }
 
     public double getTurrPos() {
-        return turrPosScalar * (robot.intake.getCurrentPosition());
+        return turrPosScalar * (robot.intake.getCurrentPosition()) + turrDefault;
 
     }
 
+    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);
@@ -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);
 
+        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);