Teste teleop

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

@@ -0,0 +1,195 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+/**
+ * Código de Teleoperado Completo para a equipe MEGA.
+ * Chassi Mecanum, Shooter, Spindexer, Feeder e Servos.
+ * Sequência: Shooter -> Wait 2s -> Servo -> Wait 0.5s -> Spindexer.
+ */
+@TeleOp(name = "Teleoperado MEGA v3", group = "TeleOp")
+public class Teleop extends LinearOpMode {
+
+    // Chassi
+    private DcMotor leftFront, leftBack, rightBack, rightFront;
+
+    // Mecanismos
+    private DcMotor spindexer, feeder, shooter;
+    private Servo servoLeft, servoRight;
+
+    // --- CONFIGURAÇÃO DE COMPENSAÇÃO MANUAL (BIAS) ---
+    private double FATOR_COMPENSACAO_STRAFE = 0.8;
+
+    // --- CONFIGURAÇÃO DOS SERVOS ---
+    private double posZeroEsquerda = 0.0;
+    private double posZeroDireita = 0.0;
+    private double SERVO_ATIVO = 0.48; // Aproximadamente 70 graus
+
+    // --- TEMPOS (TIMER) ---
+    private double TEMPO_ESPERA_SHOOTER = 2.0;    // Tempo para o shooter acelerar
+    private double TEMPO_MOVIMENTO_SERVO = 0.5;  // Tempo para o servo chegar na posição final
+
+    @Override
+    public void runOpMode() {
+
+        // Hardware Map
+        leftFront  = hardwareMap.get(DcMotor.class, "leftFront");
+        leftBack   = hardwareMap.get(DcMotor.class, "leftBack");
+        rightBack  = hardwareMap.get(DcMotor.class, "rightBack");
+        rightFront = hardwareMap.get(DcMotor.class, "rightFront");
+        spindexer  = hardwareMap.get(DcMotor.class, "Spindexer");
+        feeder     = hardwareMap.get(DcMotor.class, "feeder");
+        shooter    = hardwareMap.get(DcMotor.class, "shooter");
+        servoLeft  = hardwareMap.get(Servo.class, "servoLeft");
+        servoRight = hardwareMap.get(Servo.class, "servoRight");
+
+        // Direção dos Motores
+        leftFront.setDirection(DcMotor.Direction.REVERSE);
+        leftBack.setDirection(DcMotor.Direction.REVERSE);
+        rightFront.setDirection(DcMotor.Direction.FORWARD);
+        rightBack.setDirection(DcMotor.Direction.FORWARD);
+
+        // Direção dos Servos
+        servoLeft.setDirection(Servo.Direction.FORWARD);
+        servoRight.setDirection(Servo.Direction.FORWARD);
+
+        // Zero Power Behavior
+        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        spindexer.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        feeder.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+        telemetry.addLine("--- AJUSTE MANUAL DOS SERVOS ---");
+        telemetry.addLine("Ajuste os servos manualmente agora!");
+        telemetry.addLine("START salvará a posição como ZERO.");
+        telemetry.update();
+
+        waitForStart();
+
+        // Salvamos a posição atual como o "Zero"
+        posZeroEsquerda = servoLeft.getPosition();
+        posZeroDireita = servoRight.getPosition();
+
+        // Variáveis de Estado
+        boolean lastRT = false;
+        boolean lastLT = false;
+        double shooterPower = 0;
+        ElapsedTime timer = new ElapsedTime();
+
+        while (opModeIsActive()) {
+
+            // --- MOVIMENTAÇÃO MECANUM ---
+            double eixoY = -gamepad1.left_stick_y;
+            double eixoX = gamepad1.left_stick_x;
+            double rotacao = gamepad1.right_stick_x;
+
+            double multEsq = 1.0;
+            double multDir = 1.0;
+            if (eixoX < -0.1) multDir = 1.0 + (Math.abs(eixoX) * FATOR_COMPENSACAO_STRAFE);
+            else if (eixoX > 0.1) multEsq = 1.0 + (Math.abs(eixoX) * FATOR_COMPENSACAO_STRAFE);
+
+            double fl = (eixoY + eixoX + rotacao) * multEsq;
+            double bl = (eixoY - eixoX + rotacao) * multEsq;
+            double fr = (eixoY - eixoX - rotacao) * multDir;
+            double br = (eixoY + eixoX - rotacao) * multDir;
+
+            double max = Math.max(Math.abs(fl), Math.max(Math.abs(bl), Math.max(Math.abs(fr), Math.abs(br))));
+            if (max > 1.0) {
+                fl /= max; bl /= max; fr /= max; br /= max;
+            }
+
+            leftFront.setPower(fl);
+            leftBack.setPower(bl);
+            rightFront.setPower(fr);
+            rightBack.setPower(br);
+
+
+            // --- MECANISMOS (GAMEPAD 2) ---
+            boolean currentRT = gamepad2.right_trigger > 0.5;
+            boolean currentLT = gamepad2.left_trigger > 0.5;
+
+            // Toggle RT (Lado Direito / Negativo)
+            if (currentRT && !lastRT) {
+                if (shooterPower == -1.0) shooterPower = 0;
+                else {
+                    shooterPower = -1.0;
+                    timer.reset(); 
+                }
+            }
+            // Toggle LT (Lado Esquerdo / Positivo)
+            if (currentLT && !lastLT) {
+                if (shooterPower == 1.0) shooterPower = 0;
+                else {
+                    shooterPower = 1.0;
+                    timer.reset(); 
+                }
+            }
+            lastRT = currentRT;
+            lastLT = currentLT;
+
+            // Liga o Shooter imediatamente
+            shooter.setPower(shooterPower);
+
+            // --- LÓGICA DE SINCRONIZAÇÃO (Shooter -> Servo -> Spindexer) ---
+            if (shooterPower != 0) {
+                double tempoDecorrido = timer.seconds();
+
+                // ETAPA 1: Shooter acelerando (0s até 2.0s)
+                if (tempoDecorrido < TEMPO_ESPERA_SHOOTER) {
+                    spindexer.setPower(0);
+                    servoLeft.setPosition(posZeroEsquerda);
+                    servoRight.setPosition(posZeroDireita);
+                } 
+                // ETAPA 2: Servo se movendo (2.0s até 2.5s)
+                else if (tempoDecorrido < (TEMPO_ESPERA_SHOOTER + TEMPO_MOVIMENTO_SERVO)) {
+                    spindexer.setPower(0); // Spindexer CONTINUA PARADO
+
+                    if (shooterPower == 1.0) { // LT
+                        servoLeft.setPosition(Range.clip(posZeroEsquerda + SERVO_ATIVO, 0.0, 1.0));
+                        servoRight.setPosition(posZeroDireita);
+                    } else { // RT
+                        servoRight.setPosition(Range.clip(posZeroDireita + SERVO_ATIVO, 0.0, 1.0));
+                        servoLeft.setPosition(posZeroEsquerda);
+                    }
+                } 
+                // ETAPA 3: Tudo liberado (Após 2.5s)
+                else {
+                    spindexer.setPower(shooterPower * 0.8); // SÓ LIGA AGORA
+
+                    if (shooterPower == 1.0) {
+                        servoLeft.setPosition(Range.clip(posZeroEsquerda + SERVO_ATIVO, 0.0, 1.0));
+                    } else {
+                        servoRight.setPosition(Range.clip(posZeroDireita + SERVO_ATIVO, 0.0, 1.0));
+                    }
+                }
+            } else {
+                // RESET TOTAL IMEDIATO
+                spindexer.setPower(0);
+                servoLeft.setPosition(posZeroEsquerda);
+                servoRight.setPosition(posZeroDireita);
+            }
+
+            // FEEDER
+            if (gamepad2.right_bumper) feeder.setPower(1.0);
+            else if (gamepad2.left_bumper) feeder.setPower(-1.0);
+            else feeder.setPower(0);
+
+            // TELEMETRIA
+            telemetry.addData("Timer", "%.2f s", timer.seconds());
+            if (shooterPower != 0) {
+                if (timer.seconds() < TEMPO_ESPERA_SHOOTER) telemetry.addData("Fase", "Acelerando Shooter...");
+                else if (timer.seconds() < (TEMPO_ESPERA_SHOOTER + TEMPO_MOVIMENTO_SERVO)) telemetry.addData("Fase", "Movendo Servo...");
+                else telemetry.addData("Fase", "Lançamento Pronto!");
+            }
+            telemetry.update();
+        }
+    }
+}

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

@@ -0,0 +1,91 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+@TeleOp(name = "TELEOPERADO COMPLETO", group = "LinearOpMode")
+public class teleoperadoCompleto extends LinearOpMode {
+
+    // Variáveis para a lógica do Toggle (liga/desliga) do Shooter
+    private boolean shooterLigado = false;
+    private boolean rtPressionado = false;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        // --- MOTORES DA TRAÇÃO (PLAYER 1) ---
+        DcMotor frontLeftMotor = hardwareMap.dcMotor.get("leftFront");
+        DcMotor backLeftMotor = hardwareMap.dcMotor.get("leftBack");
+        DcMotor frontRightMotor = hardwareMap.dcMotor.get("rightFront");
+        DcMotor backRightMotor = hardwareMap.dcMotor.get("rightBack");
+
+        // --- MOTORES DO PLAYER 2 ---
+        DcMotor feederMotor = hardwareMap.dcMotor.get("feeder");
+        DcMotor shooterMotor = hardwareMap.dcMotor.get("shooter");
+
+        // Inverter os motores do lado esquerdo para manter a tração alinhada
+        frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        telemetry.addData("Status", "Inicializado");
+        telemetry.update();
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            // ==========================================
+            // PLAYER 1: MOVIMENTAÇÃO MECANUM
+            // ==========================================
+            double y = -gamepad1.left_stick_y;
+            double x = gamepad1.left_stick_x * 1.1;
+            double rx = gamepad1.right_stick_x;
+
+            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1.0);
+
+            frontLeftMotor.setPower((y + x + rx) / denominator);
+            backLeftMotor.setPower((y - x + rx) / denominator);
+            frontRightMotor.setPower((y - x - rx) / denominator);
+            backRightMotor.setPower((y + x - rx) / denominator);
+
+            // ==========================================
+            // PLAYER 2: CONTROLE DO FEEDER (LB / RB)
+            // ==========================================
+            double feederPower = 0.8;
+
+            if (gamepad2.left_bumper) {
+                feederMotor.setPower(-feederPower);
+            } else if (gamepad2.right_bumper) {
+                feederMotor.setPower(feederPower);
+            } else {
+                feederMotor.setPower(0.0);
+            }
+
+            // PLAYER 2: CONTROLE DO SHOOTER (RT - TOGGLE)
+            // Considera o gatilho pressionado se passar de 50% do curso
+            boolean rtAtual = gamepad2.right_trigger > 0.5;
+
+            // Detecta a transição: Pressionou AGORA e NÃO estava pressionado antes
+            if (rtAtual && !rtPressionado) {
+                shooterLigado = !shooterLigado; // Inverte o estado (true vira false, false vira true)
+            }
+            rtPressionado = rtAtual; // Atualiza o estado anterior do botão
+
+            // Aplica a potência no Shooter (sentido anti-horário: potência negativa)
+            double shooterPower = 1.0; // Ajuste a velocidade do shooter aqui (0.0 a 1.0)
+            if (shooterLigado) {
+                shooterMotor.setPower(-shooterPower);
+            } else {
+                shooterMotor.setPower(0.0);
+            }
+
+            // TELEMETRIA
+            telemetry.addData("P1 - Frente/Trás (Y)", y);
+            telemetry.addData("P1 - Lateral (X)", x);
+            telemetry.addData("P1 - Giro (RX)", rx);
+            telemetry.addData("P2 - Feeder Potência", feederMotor.getPower());
+            telemetry.addData("P2 - Shooter Status", shooterLigado ? "LIGADO" : "DESLIGADO");
+            telemetry.update();
+        }
+    }
+}