diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java new file mode 100644 index 000000000000..d716c751f12c --- /dev/null +++ b/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(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleoperadoCompleto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleoperadoCompleto.java new file mode 100644 index 000000000000..57af65c0540b --- /dev/null +++ b/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(); + } + } +} \ No newline at end of file