ok

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java

@@ -1,89 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-import java.util.Objects;
-
-public class Drivetrain {
-
-    private final GamepadEx gamepad;
-    private final DcMotorEx fl;
-    private final DcMotorEx fr;
-    private final DcMotorEx bl;
-    private final DcMotorEx br;
-    public MultipleTelemetry TELE;
-    private String Mode = "Default";
-    private double defaultSpeed = 0.7;
-
-    private double slowSpeed = 0.3;
-
-    public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1) {
-
-        this.fl = robot.frontLeft;
-        this.fr = robot.frontRight;
-        this.br = robot.backRight;
-        this.bl = robot.backLeft;
-
-        this.gamepad = gamepad1;
-
-        this.TELE = tele;
-
-    }
-
-    public void setMode(String mode) {
-        this.Mode = mode;
-    }
-
-    public void setDefaultSpeed(double speed) {
-        this.defaultSpeed = speed;
-    }
-
-    public void setSlowSpeed(double speed) {
-        this.slowSpeed = speed;
-    }
-
-    public void RobotCentric(double fwd, double strafe, double turn, double turbo) {
-
-        double y = -fwd; // Remember, Y stick value is reversed
-        double x = strafe * 1.1; // Counteract imperfect strafing
-        double rx = turn;
-
-        // Denominator is the largest motor power (absolute value) or 1
-        // This ensures all the powers maintain the same ratio,
-        // but only if at least one is out of the range [-1, 1]
-        double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
-        double frontLeftPower = (y + x + rx) / denominator;
-        double backLeftPower = (y - x + rx) / denominator;
-        double frontRightPower = (y - x - rx) / denominator;
-        double backRightPower = (y + x - rx) / denominator;
-
-        fl.setPower(frontLeftPower * turbo);
-        bl.setPower(backLeftPower * turbo);
-        fr.setPower(frontRightPower * turbo);
-        br.setPower(backRightPower * turbo);
-
-    }
-
-    public void update() {
-
-        if (Objects.equals(Mode, "Default")) {
-
-            RobotCentric(
-                    gamepad.getRightY(),
-                    gamepad.getRightX(),
-                    gamepad.getLeftX(),
-                    (gamepad.getTrigger(
-                            GamepadKeys.Trigger.RIGHT_TRIGGER) * (1 - defaultSpeed)
-                            - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed
-                            + defaultSpeed
-                    )
-            );
-        }
-
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java

@@ -1,162 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystems;
-
-import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
-
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-@Config
-public class Shooter {
-
-    // ================================================================
-    // -------------------  DASHBOARD CONSTANTS  -----------------------
-    // ================================================================
-
-    public static int mode = 0;            // 0 = manual, 1 = velocity PID, 2 = autoTrack
-    public static double parameter = 0.0;  // manual: power, vel: target RPM, auto: target RPM
-
-    public static double MAX_RPM = 2500;
-    public static double kP = 0.01;
-    public static double maxStep = 0.2;
-
-    public static double transferPower = 0.0;
-    public static double hoodPos = 0.501;
-
-    // ================================================================
-    // -------------   AUTO TRACK TUNING CONSTANTS  -------------------
-    // ================================================================
-
-    // z offset between shooter and goal
-    public static double dz = 30;   // inches
-
-    // Quadratic fit for shooter velocity vs distance
-    // v = A*d^2 + B*d + C
-    public static double A = 0.0004;
-    public static double B = 0.9;
-    public static double C = 1200;
-
-    // Hood angle trig model
-    // hood = HOOD_A * atan(d * HOOD_B) + HOOD_C
-    public static double HOOD_A = 0.42;
-    public static double HOOD_B = 0.012;
-    public static double HOOD_C = 0.22;
-
-    // ================================================================
-    // ----------------------  INTERNAL STATE  ------------------------
-    // ================================================================
-
-    private DcMotorEx leftShooter, rightShooter, encoder;
-
-    private double lastRevolutions = 0.0;
-    private double lastTime = 0.0;
-
-    private MultipleTelemetry TELE;
-    private Robot robot;
-
-    // ================================================================
-    // --------------------------- INIT -------------------------------
-    // ================================================================
-
-    public void init(Robot robot, MultipleTelemetry TELE) {
-        this.robot = robot;
-        this.TELE = TELE;
-
-        leftShooter = robot.shooter1;
-        rightShooter = robot.shooter2;
-        encoder = robot.shooter1;
-
-        lastTime = 0.0;
-        lastRevolutions = 0.0;
-    }
-
-    // ================================================================
-    // -------------------------- UPDATE ------------------------------
-    // ================================================================
-
-    public void update(double runtimeSec) {
-
-        double kF = 1.0 / MAX_RPM;
-
-        double rev = encoder.getCurrentPosition() / 2048.0;
-        double velocity = -60 * (rev - lastRevolutions) / (runtimeSec - lastTime);
-
-        TELE.addLine("Mode: 0=Manual, 1=Vel, 2=AutoTrack");
-        TELE.addData("Parameter", parameter);
-        TELE.addData("Velocity", velocity);
-
-        if (mode == 0) {
-            // Manual
-            leftShooter.setPower(parameter);
-            rightShooter.setPower(parameter);
-        }
-        else if (mode == 1 || mode == 2) {
-            // Velocity PID (shared logic)
-            double feed = kF * parameter;
-            double error = parameter - velocity;
-            double correction = kP * error;
-
-            correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-            double finalPower = Math.max(0, Math.min(1, feed + correction));
-
-            leftShooter.setPower(finalPower);
-            rightShooter.setPower(finalPower);
-        }
-
-        robot.hood.setPosition(hoodPos);
-        robot.transfer.setPower(transferPower);
-
-        lastTime = runtimeSec;
-        lastRevolutions = rev;
-
-        TELE.update();
-    }
-
-    // ================================================================
-    // ------------------------ AUTO TRACK ----------------------------
-    // ================================================================
-
-    public void autoTrack(Pose2d robotPose, Pose2d goalPose) {
-
-        mode = 2;  // Auto tracking → velocity PID
-
-        // Compute 3D distance
-        double dx = goalPose.position.x - robotPose.position.x;
-        double dy = goalPose.position.y - robotPose.position.y;
-
-        double distance = Math.sqrt(dx*dx + dy*dy + dz*dz);
-
-        // ---- Velocity Fit ----
-        double targetVelocity = A * distance * distance + B * distance + C;
-        parameter = targetVelocity;
-
-        // ---- Hood Fit ----
-        hoodPos = HOOD_A * Math.atan(distance * HOOD_B) + HOOD_C;
-
-        // Telemetry
-        TELE.addLine("AUTO TRACK ACTIVE");
-        TELE.addData("Distance", distance);
-        TELE.addData("Target Velocity", targetVelocity);
-        TELE.addData("Hood", hoodPos);
-    }
-
-    // ================================================================
-    // --------------------- USER CALL METHODS ------------------------
-    // ================================================================
-
-    public void setManualPower(double p) {
-        mode = 0;
-        parameter = p;
-    }
-
-    public void setVelocity(double rpm) {
-        mode = 1;
-        parameter = rpm;
-    }
-
-    public void setHood(double pos) { hoodPos = pos; }
-
-    public void setTransfer(double p) { transferPower = p; }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -1,7 +1,9 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
+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.tests.ShooterTest.*;
+import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
+import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -9,6 +11,7 @@ import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 import java.util.ArrayList;
@@ -16,16 +19,13 @@ import java.util.List;
 
 public class TeleopV2 extends LinearOpMode {
 
+    public static double vel = 3000;
+    public static double hood = 0.5;
     Robot robot;
     MultipleTelemetry TELE;
-
     boolean intake = false;
     boolean reject = false;
-    private double lastEncoderRevolutions = 0.0;
+    int ticker = 0;
-    private double lastTimeStamp = 0.0;
-
-    private double vel = 3000;
-
     List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
     List<Double> s1G = new ArrayList<>();
     List<Double> s2G = new ArrayList<>();
@@ -33,6 +33,15 @@ public class TeleopV2 extends LinearOpMode {
     List<Boolean> s1 = new ArrayList<>();
     List<Boolean> s2 = new ArrayList<>();
     List<Boolean> s3 = new ArrayList<>();
+    private double lastEncoderRevolutions = 0.0;
+    private double lastTimeStamp = 0.0;
+    private double velo1, velo;
+    private double stamp1, stamp, initPos;
+    private boolean shootAll = false;
+    double desiredTurretAngle = 180;
+
+    MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart);
+
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -50,7 +59,7 @@ public class TeleopV2 extends LinearOpMode {
         if (isStopRequested()) return;
         while (opModeIsActive()) {
 
-        //DRIVETRAIN:
+            //DRIVETRAIN:
 
             double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
             double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
@@ -67,7 +76,7 @@ public class TeleopV2 extends LinearOpMode {
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
 
-        //INTAKE:
+            //INTAKE:
 
             if (gamepad1.rightBumperWasPressed()) {
                 intake = true;
@@ -96,7 +105,7 @@ public class TeleopV2 extends LinearOpMode {
                 robot.intake.setPower(0);
             }
 
-        //COLOR:
+            //COLOR:
 
             double s1D = robot.color1.getDistance(DistanceUnit.MM);
             double s2D = robot.color2.getDistance(DistanceUnit.MM);
@@ -151,38 +160,73 @@ public class TeleopV2 extends LinearOpMode {
             boolean green2 = s2.get(s2.size() - 1);
             boolean green3 = s3.get(s3.size() - 1);
 
-        //SHOOTER:
+            //SHOOTER:
 
-            double kF = 1.0 / MAX_RPM;     // baseline feedforward
+            double penguin = 0;
 
-            double encoderRevolutions = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
+            if (ticker % 8 == 0) {
+                penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
+                double stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
+                initPos = penguin;
+                stamp1 = stamp;
+            }
 
-            double velocity = -60 * (encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp);
+            velo = velo1;
 
-            double velPID;
+            double feed = vel / 4500;
 
-            // --- FEEDFORWARD BASE POWER ---
+            if (vel > 500) {
-            double feed = kF * vel;        // Example: vel=2500 → feed=0.5
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
 
             // --- PROPORTIONAL CORRECTION ---
-            double error = vel - velocity;
+            double error = vel - velo1;
             double correction = kP * error;
 
             // limit how fast power changes (prevents oscillation)
             correction = Math.max(-maxStep, Math.min(maxStep, correction));
 
             // --- FINAL MOTOR POWER ---
-            velPID = feed + correction;
+            double powPID = feed + correction;
 
             // clamp to allowed range
-            velPID = Math.max(0, Math.min(1, velPID));
+            powPID = Math.max(0, Math.min(1, powPID));
 
-            robot.shooter1.setPower(velPID);
+            if (vel - velo > 1000) {
-            robot.shooter2.setPower(velPID);
+                powPID = 1;
+            } else if (velo - vel > 1000) {
+                powPID = 0;
+            }
+
+            robot.shooter1.setPower(powPID);
+            robot.shooter2.setPower(powPID);
+
+            robot.hood.setPosition(hood);
 
             //TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
 
-        //MISC:
+            //TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
+
+
+
+
+            //SHOOT ALL:
+
+            if (gamepad2.rightBumperWasPressed()){
+                shootAll = true;
+            }
+
+            if (shootAll){
+                intake  = false;
+                reject = false;
+
+            }
+
+            //MISC:
+
+            drive.updatePoseEstimate();
+
 
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();
@@ -192,8 +236,13 @@ public class TeleopV2 extends LinearOpMode {
             TELE.addData("Spin2Green", s2.get(s2.size() - 1));
             TELE.addData("Spin3Green", s3.get(s3.size() - 1));
 
+            TELE.addData("pose", drive.localizer.getPose());
+            TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
+
             TELE.update();
 
+            ticker++;
+
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java

@@ -6,7 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java

@@ -1,46 +0,0 @@
-package org.firstinspires.ftc.teamcode.tests;
-
-import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.firstinspires.ftc.teamcode.subsystems.Drivetrain;
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
-public class TrackingTest extends LinearOpMode {
-
-    Robot robot;
-
-    Drivetrain drivetrain;
-
-    MultipleTelemetry TELE;
-
-    GamepadEx g1;
-
-
-
-    @Override
-    public void runOpMode() throws InterruptedException {
-
-        robot = new Robot(hardwareMap);
-
-        TELE = new MultipleTelemetry(
-                telemetry, FtcDashboard.getInstance().getTelemetry()
-        );
-
-        g1 = new GamepadEx(gamepad1);
-
-        drivetrain = new Drivetrain(robot, TELE, g1);
-
-
-        waitForStart();
-        if(isStopRequested()) return;
-        while (opModeIsActive()){
-            drivetrain.update();
-        }
-
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AprilTagWebcam.java

@@ -1,14 +1,11 @@
-package org.firstinspires.ftc.teamcode.subsystems;
+package org.firstinspires.ftc.teamcode.utils;
 
 import android.util.Size;
 
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;