Merging with latest Pedro pathing

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Configuration

@@ -0,0 +1,27 @@
+Control Hub:
+
+Motors:
+0: leftFront
+1: leftRear
+2: rightFront
+3: rightRear
+
+Servos:
+5: turretServo
+
+Expansion Hub:
+
+Servo:
+0: shooterLight
+
+Motors:
+1: transferMotor
+2: intakeMotor
+3: shooterMotor
+
+
+
+shooter close = 1000
+shooter mid = 1125
+shooter far = 1375
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Intake.java

@@ -0,0 +1,67 @@
+package org.firstinspires.ftc.teamcode;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+
+public class Intake {
+    public DcMotorEx intakeMotor;
+    public DcMotorEx transferMotor;
+    private final double INTAKE_POWER = 0.75;
+
+    private final double SHOOTING_POWER = 0.5;
+
+    private final double INTAKE_OFF = 0.0;
+
+    private final double TRANSFER_INTAKE_POWER = -0.4;
+
+    private final double TRANFER_SHOOTING_POWER = -0.75;
+
+    private final double TRANSFER_OFF = 0.0;
+
+
+
+    public Intake (HardwareMap hardwareMap) {
+        intakeMotor = hardwareMap.get(DcMotorEx.class,"intakeMotor");
+        intakeMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+        intakeMotor.setDirection(DcMotorEx.Direction.FORWARD);
+
+        transferMotor = hardwareMap.get(DcMotorEx.class, "transferMotor");
+        transferMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+        transferMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+    }
+    public void startIntakeAndTransfer() {
+        intakeMotor.setPower(INTAKE_POWER);
+        transferMotor.setPower(TRANSFER_INTAKE_POWER);
+    }
+
+    public void startTransferOnly() {
+        transferMotor.setPower(TRANSFER_INTAKE_POWER);
+    }
+
+    public void startIntakeOnly() {
+        intakeMotor.setPower(INTAKE_POWER);
+    }
+
+    public void shootArtifacts() {
+        intakeMotor.setPower(SHOOTING_POWER);
+        transferMotor.setPower(TRANFER_SHOOTING_POWER);
+    }
+    public void intakeStop(){
+        intakeMotor.setPower(0);
+        transferMotor.setPower(0);
+    }
+    public void transferOnly() {
+        intakeMotor.setPower(0);
+        transferMotor.setPower(0.75);
+    }
+    public void stopIntake() {
+        intakeMotor.setPower(INTAKE_OFF);
+    }
+
+    public void stopTransfer() {
+        transferMotor.setPower(TRANSFER_OFF);
+    }
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java

@@ -0,0 +1,29 @@
+package org.firstinspires.ftc.teamcode;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.Intake;
+import org.firstinspires.ftc.teamcode.Shooter;
+import org.firstinspires.ftc.teamcode.Turret;
+
+public class Robot {
+
+    public static final int BLUE_TARGET_TAG_ID = 20;
+    public static final int PIPELINE_ID_BLUE = 8;
+    public static final int RED_TARGET_TAG_ID = 24;
+    public static final int PIPELINE_ID_RED = 2;
+    public static int current_pipeline_id = PIPELINE_ID_RED;
+    public static int current_tag_id = RED_TARGET_TAG_ID;
+
+    public Intake intake;
+    public Shooter shooter;
+    public Turret turret;
+    public HardwareMap hardwareMap;
+    public Telemetry telemetry;
+    public Robot (HardwareMap hardwareMap, Telemetry telemetry) {
+        this.hardwareMap = hardwareMap;
+        intake = new Intake(hardwareMap);
+        shooter = new Shooter(hardwareMap);
+        turret = new Turret(hardwareMap);
+        this.telemetry = telemetry;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTeleop.java

@@ -0,0 +1,241 @@
+package org.firstinspires.ftc.teamcode;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+
+import com.pedropathing.util.Timer;
+
+
+/**
+ * Standard Robot TeleOp for FTC using Pedro Pathing.
+ * Handles robot driving, shooter motor control, and turret control (CRServo).
+ *
+ * Author:
+ *   Baron Henderson – 20077 The Indubitables (modified by Kushal Madhabhaktula)
+ * Version:
+ *   3.1, 10/2025
+ */
+@TeleOp(name = "RobotTeleop", group = "Competition")
+public class RobotTeleop extends OpMode {
+    private Timer timer;
+    private Timer rapidTimer;
+
+    private Follower follower;
+    private Robot robot;
+
+    private Vision vision;
+    private TurretTracker turretTracker;
+    private static final double DEAD_ZONE = 0.1;
+    private static final double TURRET_DEADZONE = 0.3; // Tighter alignment threshold
+
+    private final Pose startPose = new Pose(0, 0, 0);
+
+    private Pose currentPose = new Pose(0,0,0);
+
+    private boolean is_RapidFireOn = false;
+    private boolean targetTracking_enabled = true;
+
+    private String currentAlliance = "RED";
+
+
+    @Override
+    public void init() {
+
+        timer = new Timer();
+        rapidTimer = new Timer();
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(startPose);
+        follower.startTeleopDrive();
+        robot = new Robot(hardwareMap, telemetry);
+        if(gamepad1.y) {
+            Robot.current_pipeline_id = Robot.PIPELINE_ID_BLUE;
+            Robot.current_tag_id = Robot.BLUE_TARGET_TAG_ID;
+            currentAlliance = "BLUE";
+        }
+        else{
+            Robot.current_pipeline_id = Robot.PIPELINE_ID_RED;
+            Robot.current_tag_id = Robot.RED_TARGET_TAG_ID;
+        }
+//        turretTracker = new TurretTracker(robot);
+        vision = new Vision(hardwareMap, robot);
+        telemetry.addData("Current Alliance: ", currentAlliance);
+        telemetry.addLine("RobotTeleop Initialized (CRServo turret)");
+        telemetry.update();
+    }
+
+    @Override
+    public void start() {
+        timer.resetTimer();
+        follower.startTeleopDrive();
+        follower.setMaxPower(1.0);
+    }
+
+//    private boolean is_CloseShot() {return gamepad2.b; }
+
+    private boolean is_FarShot() {
+        return gamepad2.x;
+    }
+
+    private boolean is_Intaking() {
+        return gamepad2.left_bumper;
+    }
+
+    private boolean is_Shooting() {
+        return gamepad2.right_bumper;
+    }
+
+    private boolean is_HumanPlayer() {
+        return gamepad1.a;
+    }
+
+    private boolean is_FlywheelOff() {
+        return gamepad2.a;
+    }
+
+//    private boolean is_TurretLeft() {
+//        return gamepad2.dpad_left;
+//    }
+//
+//    private boolean is_TurretRight() {
+//        return gamepad2.dpad_right;
+//    }
+
+    private boolean is_ShootingRapidFireCloseRange() {return gamepad2.b;}
+
+    private boolean is_ShootingRapidFireMidRange() {
+        return gamepad2.y;
+    }
+
+
+
+    @Override
+    public void loop() {
+
+        double xInput = Math.abs(gamepad1.left_stick_x) > DEAD_ZONE ? -gamepad1.left_stick_x : 0;
+        double yInput = Math.abs(gamepad1.left_stick_y) > DEAD_ZONE ? -gamepad1.left_stick_y : 0;
+        // NOTE: rotation is negated to match PedroPathing's TeleOp example (prevents reversed/odd rotation behavior)
+        double turnInput = Math.abs(gamepad1.right_stick_x) > DEAD_ZONE ? -gamepad1.right_stick_x : 0;
+
+        double powerScale = gamepad1.right_trigger > 0.5 ? 0.25 : 1.0;
+
+        follower.updateErrors();
+        follower.updateVectors();
+        follower.setTeleOpDrive(
+                yInput * powerScale,  // forward/backward
+                xInput * powerScale,  // strafe
+                turnInput * powerScale, // rotation (negated)
+                true                     // robot-centric
+        );
+
+        follower.update();
+        if (targetTracking_enabled) {
+            vision.update();
+        }
+
+        if (is_FarShot()) {
+            robot.shooter.startFarShoot();
+        } else if (is_ShootingRapidFireCloseRange()) {
+            robot.shooter.startCloseShoot();
+        } else if (is_HumanPlayer()) {
+            robot.shooter.startHumanIntake();
+        } else if (is_FlywheelOff()){
+            robot.shooter.stopShoot();
+        }
+
+         if (is_Intaking()) {
+            robot.intake.startIntakeOnly();
+        } else {
+            robot.intake.stopIntake();
+         }
+
+        if (is_ShootingRapidFireMidRange() && !is_RapidFireOn) {
+            is_RapidFireOn = true;
+            robot.shooter.startMidShoot();
+            rapidTimer.resetTimer();
+        }
+        if (is_RapidFireOn) {
+            if (robot.shooter.reachedSpeed()) {
+                robot.intake.shootArtifacts();
+                gamepad1.rumble(1000);
+                gamepad2.rumble(1000);
+            }
+            if (rapidTimer.getElapsedTime() >= 5750) {
+                robot.shooter.stopFlyWheel();
+                robot.intake.intakeStop();
+                robot.intake.stopTransfer();
+                is_RapidFireOn = false;
+            }
+        } else {
+            if (is_Shooting()) {
+                if (robot.shooter.reachedSpeed()) {
+                    robot.intake.startTransferOnly();
+                    gamepad1.rumble(1000);
+                    gamepad2.rumble(1000);
+                }
+            } else {
+                robot.intake.stopTransfer();
+            }
+        }
+
+        if (is_ShootingRapidFireCloseRange() && !is_RapidFireOn) {
+            is_RapidFireOn = true;
+            robot.shooter.startCloseShoot();
+            rapidTimer.resetTimer();
+        }
+        if (is_RapidFireOn) {
+            if (robot.shooter.reachedSpeed()) {
+                robot.intake.shootArtifacts();
+                gamepad1.rumble(1000);
+                gamepad2.rumble(1000);
+            }
+            if (rapidTimer.getElapsedTime() >= 5750) {
+                robot.shooter.stopFlyWheel();
+                robot.intake.intakeStop();
+                robot.intake.stopTransfer();
+                is_RapidFireOn = false;
+            }
+        } else {
+            if (is_Shooting()) {
+                if (robot.shooter.reachedSpeed()) {
+                    robot.intake.startTransferOnly();
+                    gamepad1.rumble(1000);
+                    gamepad2.rumble(1000);
+                }
+            } else {
+                robot.intake.stopTransfer();
+            }
+        }
+
+
+        // Turret control (fixed: check gamepad2 on both dpad sides)
+        if (gamepad2.dpad_right && !gamepad2.dpad_left) {
+            robot.turret.goRight(); // rotate right
+        } else if (gamepad2.dpad_left && !gamepad2.dpad_right) {
+            robot.turret.goLeft(); // rotate left
+        } else {
+            robot.turret.stopTurret();
+        }
+
+        currentPose = follower.getPose();
+        robot.shooter.shooterLightUpdate();
+        telemetry.addData("Current Alliance: ", currentAlliance);
+        telemetry.addData("Rapid Fire On: ", is_RapidFireOn);
+        telemetry.addData("Drive X", xInput);
+        telemetry.addData("Drive Y", yInput);
+        telemetry.addData("Turn", turnInput);
+        //telemetry.addData("Turret Power", turretCR.getPower());
+        telemetry.addData("Pose X", follower.getPose().getX());
+        telemetry.addData("Pose Y", follower.getPose().getY());
+        telemetry.addData("Heading (deg)", Math.toDegrees(follower.getPose().getHeading()));
+        telemetry.update();
+    }
+
+    @Override
+    public void stop() {
+        robot.shooter.stopShoot();
+        robot.intake.stopIntake();
+        robot.intake.stopTransfer();
+    }
+}