From 1b461bd21fd6fa32432d072db4febaa8a00d6881 Mon Sep 17 00:00:00 2001 From: SESI Mega Snakes FTC Date: Tue, 14 Jul 2026 16:52:58 -0300 Subject: [PATCH 1/4] tentativa add IMU --- .../firstinspires/ftc/teamcode/Teleop.java | 746 ++++++++++++++++++ 1 file changed, 746 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java 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..e2eeb6c8aa16 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -0,0 +1,746 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +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.IMU; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +@TeleOp(name = "Teleoperado IMU", group = "TeleOp") +public class Teleop extends LinearOpMode { + + // ========================================================= + // MOTORES DO CHASSI + // ========================================================= + + private DcMotor leftFront; + private DcMotor leftBack; + private DcMotor rightFront; + private DcMotor rightBack; + + // ========================================================= + // MECANISMOS + // ========================================================= + + private DcMotor shooter; + private DcMotor feeder; + private DcMotor spindexer; + + // ========================================================= + // IMU + // ========================================================= + + private IMU imu; + + // Ângulo que o robô deve manter durante o strafe + private double targetHeading = 0.0; + + // Indica se o Heading Lock está ativo + private boolean headingLockActive = false; + + // ========================================================= + // CONTROLE DA CORREÇÃO DO STRAFE + // ========================================================= + + /* + * P = força principal da correção. + * + * Antes estava em 0.020. + * Agora está mais forte para corrigir rapidamente. + */ + private static final double HEADING_KP = 0.15; + + /* + * D = reage quando o robô começa a girar rapidamente. + * + * Ajuda a corrigir antes de o erro ficar muito grande. + */ + private static final double HEADING_KD = 0.02; + + // Limite máximo da correção de giro + private static final double MAX_HEADING_CORRECTION = 10; + + // Erro anterior usado pelo D + private double lastHeadingError = 0.0; + + // Cronômetro para calcular o D + private final ElapsedTime headingTimer = new ElapsedTime(); + + // ========================================================= + // SHOOTER + // ========================================================= + + private double shooterPower = 0.0; + + private boolean lastRightTrigger = false; + private boolean lastLeftTrigger = false; + + @Override + public void runOpMode() { + + // ===================================================== + // MAPEAMENTO DOS MOTORES + // ===================================================== + + leftFront = + hardwareMap.get(DcMotor.class, "leftFront"); + + leftBack = + hardwareMap.get(DcMotor.class, "leftBack"); + + rightFront = + hardwareMap.get(DcMotor.class, "rightFront"); + + rightBack = + hardwareMap.get(DcMotor.class, "rightBack"); + + + shooter = + hardwareMap.get(DcMotor.class, "shooter"); + + feeder = + hardwareMap.get(DcMotor.class, "feeder"); + + spindexer = + hardwareMap.get(DcMotor.class, "Spindexer"); + + // ===================================================== + // CONFIGURAÇÃO DA IMU + // + // Logo REV = LEFT + // USB = UP + // ===================================================== + + imu = hardwareMap.get(IMU.class, "imu"); + + RevHubOrientationOnRobot hubOrientation = + new RevHubOrientationOnRobot( + + RevHubOrientationOnRobot + .LogoFacingDirection.LEFT, + + RevHubOrientationOnRobot + .UsbFacingDirection.UP + ); + + imu.initialize( + new IMU.Parameters(hubOrientation) + ); + + // ===================================================== + // DIREÇÃO DOS MOTORES + // ===================================================== + + leftFront.setDirection( + DcMotor.Direction.REVERSE + ); + + leftBack.setDirection( + DcMotor.Direction.REVERSE + ); + + rightFront.setDirection( + DcMotor.Direction.FORWARD + ); + + rightBack.setDirection( + DcMotor.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 + ); + + shooter.setZeroPowerBehavior( + DcMotor.ZeroPowerBehavior.BRAKE + ); + + feeder.setZeroPowerBehavior( + DcMotor.ZeroPowerBehavior.BRAKE + ); + + spindexer.setZeroPowerBehavior( + DcMotor.ZeroPowerBehavior.BRAKE + ); + + // Tudo começa parado + stopAllMotors(); + + telemetry.addLine("TeleOp pronto"); + telemetry.addLine("Pressione START"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) { + return; + } + + // Zera o ângulo atual da IMU + imu.resetYaw(); + + while (opModeIsActive()) { + + controlMecanum(); + + controlShooterAndSpindexer(); + + controlFeeder(); + + telemetry.addData( + "Heading", + "%.1f graus", + getHeading() + ); + + telemetry.addData( + "IMU no strafe", + headingLockActive + ? "ATIVA" + : "DESATIVADA" + ); + + telemetry.addData( + "Angulo alvo", + "%.1f graus", + targetHeading + ); + + telemetry.addData( + "Shooter", + "%.1f", + shooterPower + ); + + telemetry.update(); + } + + stopAllMotors(); + } + + // ========================================================= + // CONTROLE MECANUM + // ========================================================= + + private void controlMecanum() { + + // ===================================================== + // LEITURA DOS ANALÓGICOS + // ===================================================== + + // Frente / trás + double y = + -gamepad1.left_stick_y; + + /* + * STRAFE + * + * O sinal foi invertido porque no seu robô: + * + * Analógico esquerda -> robô ia para direita + * Analógico direita -> robô ia para esquerda + */ + double x = + -gamepad1.left_stick_x; + + // Giro manual + double manualRotation = + gamepad1.right_stick_x; + + // ===================================================== + // DEADZONE + // ===================================================== + + y = applyDeadzone(y, 0.08); + + x = applyDeadzone(x, 0.08); + + manualRotation = + applyDeadzone( + manualRotation, + 0.08 + ); + + // ===================================================== + // DETECTA STRAFE PURO + // ===================================================== + + boolean pureStrafe = + + Math.abs(x) > 0.15 + + && Math.abs(y) < 0.15 + + && Math.abs(manualRotation) < 0.15; + + double rotation; + + // ===================================================== + // STRAFE COM HEADING LOCK + // ===================================================== + + if (pureStrafe) { + + /* + * Essa parte acontece apenas UMA VEZ + * quando o strafe começa. + */ + if (!headingLockActive) { + + // Salva o ângulo inicial + targetHeading = + getHeading(); + + headingLockActive = + true; + + // Reinicia o controlador + lastHeadingError = + 0.0; + + headingTimer.reset(); + } + + // Ângulo atual + double currentHeading = + getHeading(); + + // ================================================= + // ERRO DE ÂNGULO + // ================================================= + + double headingError = + normalizeAngle( + currentHeading + - targetHeading + ); + + // ================================================= + // TEMPO ENTRE OS LOOPS + // ================================================= + + double deltaTime = + headingTimer.seconds(); + + headingTimer.reset(); + + // Evita divisão por zero + if (deltaTime < 0.001) { + deltaTime = 0.001; + } + + // ================================================= + // DERIVADA + // ================================================= + + double derivative = + ( + headingError + - lastHeadingError + ) + / deltaTime; + + // ================================================= + // CONTROLE PD + // ================================================= + + rotation = + + (headingError * HEADING_KP) + + + + + (derivative * HEADING_KD); + + // Salva o erro atual + lastHeadingError = + headingError; + + // Limita a correção + rotation = + clamp( + + rotation, + + -MAX_HEADING_CORRECTION, + + MAX_HEADING_CORRECTION + ); + + } else { + + // ================================================= + // FORA DO STRAFE + // ================================================= + + headingLockActive = + false; + + // Piloto controla o giro normalmente + rotation = + manualRotation; + } + + // ===================================================== + // CÁLCULO MECANUM + // ===================================================== + + double frontLeftPower = + + y + + x + + rotation; + + + double backLeftPower = + + y + - x + + rotation; + + + double frontRightPower = + + y + - x + - rotation; + + + double backRightPower = + + y + + x + - rotation; + + // ===================================================== + // NORMALIZAÇÃO + // ===================================================== + + double denominator = + + Math.max( + + Math.abs(frontLeftPower), + + Math.max( + + Math.abs(backLeftPower), + + Math.max( + + Math.abs(frontRightPower), + + Math.abs(backRightPower) + + ) + ) + ); + + denominator = + Math.max( + denominator, + 1.0 + ); + + frontLeftPower /= + denominator; + + backLeftPower /= + denominator; + + frontRightPower /= + denominator; + + backRightPower /= + denominator; + + // ===================================================== + // ENVIA POTÊNCIA PARA OS MOTORES + // ===================================================== + + leftFront.setPower( + frontLeftPower + ); + + leftBack.setPower( + backLeftPower + ); + + rightFront.setPower( + frontRightPower + ); + + rightBack.setPower( + backRightPower + ); + } + + // ========================================================= + // SHOOTER E SPINDEXER + // ========================================================= + + private void controlShooterAndSpindexer() { + + boolean currentRightTrigger = + + gamepad2.right_trigger + > 0.5; + + + boolean currentLeftTrigger = + + gamepad2.left_trigger + > 0.5; + + + // ===================================================== + // RIGHT TRIGGER + // ===================================================== + + if ( + currentRightTrigger + + && + + !lastRightTrigger + ) { + + if (shooterPower == 1.0) { + + shooterPower = + 0.0; + + } else { + + shooterPower = + 1.0; + } + } + + // ===================================================== + // LEFT TRIGGER + // ===================================================== + + if ( + currentLeftTrigger + + && + + !lastLeftTrigger + ) { + + if (shooterPower == -1.0) { + + shooterPower = + 0.0; + + } else { + + shooterPower = + -1.0; + } + } + + // Atualiza estado dos triggers + + lastRightTrigger = + currentRightTrigger; + + lastLeftTrigger = + currentLeftTrigger; + + + // Shooter + + shooter.setPower( + shooterPower + ); + + + // Spindexer + + spindexer.setPower( + shooterPower * 0.8 + ); + } + + // ========================================================= + // FEEDER + // ========================================================= + + private void controlFeeder() { + + if (gamepad2.right_bumper) { + + feeder.setPower( + 1.0 + ); + + } else if (gamepad2.left_bumper) { + + feeder.setPower( + -1.0 + ); + + } else { + + feeder.setPower( + 0.0 + ); + } + } + + // ========================================================= + // LEITURA DA IMU + // ========================================================= + + private double getHeading() { + + return imu + + .getRobotYawPitchRollAngles() + + .getYaw( + AngleUnit.DEGREES + ); + } + + // ========================================================= + // NORMALIZAÇÃO DO ÂNGULO + // ========================================================= + + private double normalizeAngle( + double angle + ) { + + while (angle > 180.0) { + + angle -= + 360.0; + } + + while (angle < -180.0) { + + angle += + 360.0; + } + + return angle; + } + + // ========================================================= + // DEADZONE + // ========================================================= + + private double applyDeadzone( + double value, + double deadzone + ) { + + if ( + Math.abs(value) + < deadzone + ) { + + return 0.0; + } + + return value; + } + + // ========================================================= + // LIMITADOR + // ========================================================= + + private double clamp( + double value, + double minimum, + double maximum + ) { + + return Math.max( + + minimum, + + Math.min( + maximum, + value + ) + ); + } + + // ========================================================= + // PARA TODOS OS MOTORES + // ========================================================= + + private void stopAllMotors() { + + if (leftFront != null) { + + leftFront.setPower( + 0.0 + ); + } + + if (leftBack != null) { + + leftBack.setPower( + 0.0 + ); + } + + if (rightFront != null) { + + rightFront.setPower( + 0.0 + ); + } + + if (rightBack != null) { + + rightBack.setPower( + 0.0 + ); + } + + if (shooter != null) { + + shooter.setPower( + 0.0 + ); + } + + if (feeder != null) { + + feeder.setPower( + 0.0 + ); + } + + if (spindexer != null) { + + spindexer.setPower( + 0.0 + ); + } + } +} \ No newline at end of file From cb379f00c9bcd765b7fbea324b39aa57ff06ad91 Mon Sep 17 00:00:00 2001 From: SESI Mega Snakes FTC Date: Wed, 29 Jul 2026 17:01:47 -0300 Subject: [PATCH 2/4] add servos --- .../firstinspires/ftc/teamcode/Teleop.java | 854 +++--------------- 1 file changed, 146 insertions(+), 708 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java index e2eeb6c8aa16..9afb514e7ac2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -1,746 +1,184 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; 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.IMU; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - -@TeleOp(name = "Teleoperado IMU", group = "TeleOp") +/** + * Código de Teleoperado para a equipe MEGA. + * Chassi Mecanum sem Sensores (Sem IMU). + * Inclui Compensação de Strafe (Bias), Shooter, Spindexer, Feeder e Servos com Delay. + */ +@TeleOp(name = "Teleoperado", group = "TeleOp") public class Teleop extends LinearOpMode { - // ========================================================= - // MOTORES DO CHASSI - // ========================================================= - - private DcMotor leftFront; - private DcMotor leftBack; - private DcMotor rightFront; - private DcMotor rightBack; - - // ========================================================= - // MECANISMOS - // ========================================================= - - private DcMotor shooter; - private DcMotor feeder; - private DcMotor spindexer; - - // ========================================================= - // IMU - // ========================================================= - - private IMU imu; - - // Ângulo que o robô deve manter durante o strafe - private double targetHeading = 0.0; - - // Indica se o Heading Lock está ativo - private boolean headingLockActive = false; + // Chassi (nomes de motores mantidos em inglês) + private DcMotor leftFront, leftBack, rightBack, rightFront; - // ========================================================= - // CONTROLE DA CORREÇÃO DO STRAFE - // ========================================================= + // Mecanismos (nomes de motores mantidos em inglês) + private DcMotor spindexer, feeder, shooter; + private Servo servoEsquerdo, servoDireito; - /* - * P = força principal da correção. - * - * Antes estava em 0.020. - * Agora está mais forte para corrigir rapidamente. - */ - private static final double HEADING_KP = 0.15; + // --- CONFIGURAÇÃO DE COMPENSAÇÃO MANUAL (BIAS) --- + private double FATOR_COMPENSACAO_STRAFE = 0.8; - /* - * D = reage quando o robô começa a girar rapidamente. - * - * Ajuda a corrigir antes de o erro ficar muito grande. - */ - private static final double HEADING_KD = 0.02; - - // Limite máximo da correção de giro - private static final double MAX_HEADING_CORRECTION = 10; - - // Erro anterior usado pelo D - private double lastHeadingError = 0.0; - - // Cronômetro para calcular o D - private final ElapsedTime headingTimer = new ElapsedTime(); - - // ========================================================= - // SHOOTER - // ========================================================= - - private double shooterPower = 0.0; - - private boolean lastRightTrigger = false; - private boolean lastLeftTrigger = false; + // --- CONFIGURAÇÃO DOS SERVOS --- + // Posições (0.0 a 1.0). Ajuste conforme a montagem do seu robô. + private double SERVO_INATIVO = 0.0; // Posição inicial (recolhido) + private double SERVO_ATIVO = 0.55; // Posição de 100 graus (aproximadamente 0.55 na maioria dos servos) + private double TEMPO_ACELERACAO = 1.0; // Tempo em segundos para o motor atingir o RPM máximo @Override public void runOpMode() { - // ===================================================== - // MAPEAMENTO DOS MOTORES - // ===================================================== - - leftFront = - hardwareMap.get(DcMotor.class, "leftFront"); - - leftBack = - hardwareMap.get(DcMotor.class, "leftBack"); - - rightFront = - hardwareMap.get(DcMotor.class, "rightFront"); - - rightBack = - hardwareMap.get(DcMotor.class, "rightBack"); - - - shooter = - hardwareMap.get(DcMotor.class, "shooter"); - - feeder = - hardwareMap.get(DcMotor.class, "feeder"); - - spindexer = - hardwareMap.get(DcMotor.class, "Spindexer"); - - // ===================================================== - // CONFIGURAÇÃO DA IMU - // - // Logo REV = LEFT - // USB = UP - // ===================================================== - - imu = hardwareMap.get(IMU.class, "imu"); - - RevHubOrientationOnRobot hubOrientation = - new RevHubOrientationOnRobot( - - RevHubOrientationOnRobot - .LogoFacingDirection.LEFT, - - RevHubOrientationOnRobot - .UsbFacingDirection.UP - ); - - imu.initialize( - new IMU.Parameters(hubOrientation) - ); - - // ===================================================== - // DIREÇÃO DOS MOTORES - // ===================================================== - - leftFront.setDirection( - DcMotor.Direction.REVERSE - ); - - leftBack.setDirection( - DcMotor.Direction.REVERSE - ); - - rightFront.setDirection( - DcMotor.Direction.FORWARD - ); - - rightBack.setDirection( - DcMotor.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 - ); - - shooter.setZeroPowerBehavior( - DcMotor.ZeroPowerBehavior.BRAKE - ); - - feeder.setZeroPowerBehavior( - DcMotor.ZeroPowerBehavior.BRAKE - ); - - spindexer.setZeroPowerBehavior( - DcMotor.ZeroPowerBehavior.BRAKE - ); - - // Tudo começa parado - stopAllMotors(); - - telemetry.addLine("TeleOp pronto"); - telemetry.addLine("Pressione START"); + // Mapeamento de Hardware + 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"); + + servoEsquerdo = hardwareMap.get(Servo.class, "servoLeft"); + servoDireito = 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); + + // Comportamento Zero Power + 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); + + // Variáveis de Estado + boolean ultimoGatilhoDireito = false; + boolean ultimoGatilhoEsquerdo = false; + double shooterActivePower = 0; + + // Timer para o delay dos servos + ElapsedTime shooterTimer = new ElapsedTime(); + boolean aguardandoAceleracao = false; + + telemetry.addLine("Pronto! (Sem IMU)"); telemetry.update(); - waitForStart(); + // Inicia servos na posição idle + servoEsquerdo.setPosition(SERVO_INATIVO); + servoDireito.setPosition(SERVO_INATIVO); - if (isStopRequested()) { - return; - } - - // Zera o ângulo atual da IMU - imu.resetYaw(); + waitForStart(); while (opModeIsActive()) { - controlMecanum(); - - controlShooterAndSpindexer(); - - controlFeeder(); - - telemetry.addData( - "Heading", - "%.1f graus", - getHeading() - ); - - telemetry.addData( - "IMU no strafe", - headingLockActive - ? "ATIVA" - : "DESATIVADA" - ); - - telemetry.addData( - "Angulo alvo", - "%.1f graus", - targetHeading - ); - - telemetry.addData( - "Shooter", - "%.1f", - shooterPower - ); - - telemetry.update(); - } - - stopAllMotors(); - } + // --- CONTROLE DE MOVIMENTAÇÃO (Gamepad 1) --- + double eixoY = -gamepad1.left_stick_y; + double eixoX = gamepad1.left_stick_x; + double rotacao = gamepad1.right_stick_x; - // ========================================================= - // CONTROLE MECANUM - // ========================================================= + double multiplicadorEsquerdo = 1.0; + double multiplicadorDireito = 1.0; - private void controlMecanum() { - - // ===================================================== - // LEITURA DOS ANALÓGICOS - // ===================================================== - - // Frente / trás - double y = - -gamepad1.left_stick_y; - - /* - * STRAFE - * - * O sinal foi invertido porque no seu robô: - * - * Analógico esquerda -> robô ia para direita - * Analógico direita -> robô ia para esquerda - */ - double x = - -gamepad1.left_stick_x; - - // Giro manual - double manualRotation = - gamepad1.right_stick_x; - - // ===================================================== - // DEADZONE - // ===================================================== - - y = applyDeadzone(y, 0.08); - - x = applyDeadzone(x, 0.08); - - manualRotation = - applyDeadzone( - manualRotation, - 0.08 - ); - - // ===================================================== - // DETECTA STRAFE PURO - // ===================================================== - - boolean pureStrafe = - - Math.abs(x) > 0.15 - - && Math.abs(y) < 0.15 - - && Math.abs(manualRotation) < 0.15; - - double rotation; - - // ===================================================== - // STRAFE COM HEADING LOCK - // ===================================================== - - if (pureStrafe) { - - /* - * Essa parte acontece apenas UMA VEZ - * quando o strafe começa. - */ - if (!headingLockActive) { - - // Salva o ângulo inicial - targetHeading = - getHeading(); - - headingLockActive = - true; - - // Reinicia o controlador - lastHeadingError = - 0.0; - - headingTimer.reset(); + if (eixoX < -0.1) { + multiplicadorDireito = 1.0 + (Math.abs(eixoX) * FATOR_COMPENSACAO_STRAFE); + } else if (eixoX > 0.1) { + multiplicadorEsquerdo = 1.0 + (Math.abs(eixoX) * FATOR_COMPENSACAO_STRAFE); } - // Ângulo atual - double currentHeading = - getHeading(); - - // ================================================= - // ERRO DE ÂNGULO - // ================================================= + double fl = (eixoY + eixoX + rotacao) * multiplicadorEsquerdo; + double bl = (eixoY - eixoX + rotacao) * multiplicadorEsquerdo; + double fr = (eixoY - eixoX - rotacao) * multiplicadorDireito; + double br = (eixoY + eixoX - rotacao) * multiplicadorDireito; - double headingError = - normalizeAngle( - currentHeading - - targetHeading - ); - - // ================================================= - // TEMPO ENTRE OS LOOPS - // ================================================= - - double deltaTime = - headingTimer.seconds(); - - headingTimer.reset(); - - // Evita divisão por zero - if (deltaTime < 0.001) { - deltaTime = 0.001; + double potenciaMaxima = Math.max(Math.abs(fl), Math.max(Math.abs(bl), Math.max(Math.abs(fr), Math.abs(br)))); + if (potenciaMaxima > 1.0) { + fl /= potenciaMaxima; bl /= potenciaMaxima; fr /= potenciaMaxima; br /= potenciaMaxima; } - // ================================================= - // DERIVADA - // ================================================= - - double derivative = - ( - headingError - - lastHeadingError - ) - / deltaTime; - - // ================================================= - // CONTROLE PD - // ================================================= - - rotation = - - (headingError * HEADING_KP) - - + - - (derivative * HEADING_KD); - - // Salva o erro atual - lastHeadingError = - headingError; - - // Limita a correção - rotation = - clamp( - - rotation, - - -MAX_HEADING_CORRECTION, - - MAX_HEADING_CORRECTION - ); - - } else { - - // ================================================= - // FORA DO STRAFE - // ================================================= - - headingLockActive = - false; - - // Piloto controla o giro normalmente - rotation = - manualRotation; - } - - // ===================================================== - // CÁLCULO MECANUM - // ===================================================== - - double frontLeftPower = - - y - + x - + rotation; - - - double backLeftPower = - - y - - x - + rotation; - - - double frontRightPower = - - y - - x - - rotation; - - - double backRightPower = - - y - + x - - rotation; - - // ===================================================== - // NORMALIZAÇÃO - // ===================================================== - - double denominator = - - Math.max( - - Math.abs(frontLeftPower), - - Math.max( - - Math.abs(backLeftPower), - - Math.max( - - Math.abs(frontRightPower), - - Math.abs(backRightPower) - - ) - ) - ); - - denominator = - Math.max( - denominator, - 1.0 - ); - - frontLeftPower /= - denominator; - - backLeftPower /= - denominator; - - frontRightPower /= - denominator; - - backRightPower /= - denominator; - - // ===================================================== - // ENVIA POTÊNCIA PARA OS MOTORES - // ===================================================== - - leftFront.setPower( - frontLeftPower - ); - - leftBack.setPower( - backLeftPower - ); - - rightFront.setPower( - frontRightPower - ); - - rightBack.setPower( - backRightPower - ); - } - - // ========================================================= - // SHOOTER E SPINDEXER - // ========================================================= - - private void controlShooterAndSpindexer() { - - boolean currentRightTrigger = - - gamepad2.right_trigger - > 0.5; - - - boolean currentLeftTrigger = - - gamepad2.left_trigger - > 0.5; - - - // ===================================================== - // RIGHT TRIGGER - // ===================================================== - - if ( - currentRightTrigger - - && - - !lastRightTrigger - ) { - - if (shooterPower == 1.0) { - - shooterPower = - 0.0; - - } else { - - shooterPower = - 1.0; + leftFront.setPower(fl); + leftBack.setPower(bl); + rightFront.setPower(fr); + rightBack.setPower(br); + + + // --- CONTROLE DO SHOOTER, SPINDEXER E SERVOS (Gamepad 2) --- + boolean gatilhoDireitoAtual = gamepad2.right_trigger > 0.5; + boolean gatilhoEsquerdoAtual = gamepad2.left_trigger > 0.5; + + // Lógica de Toggle + if (gatilhoDireitoAtual && !ultimoGatilhoDireito) { + if (shooterActivePower == -1.0) { + shooterActivePower = 0; + aguardandoAceleracao = false; + } else { + shooterActivePower = -1.0; + shooterTimer.reset(); // Inicia contagem para spin-up + aguardandoAceleracao = true; + } + } + if (gatilhoEsquerdoAtual && !ultimoGatilhoEsquerdo) { + if (shooterActivePower == 1.0) { + shooterActivePower = 0; + aguardandoAceleracao = false; + } else { + shooterActivePower = 1.0; + shooterTimer.reset(); // Inicia contagem para spin-up + aguardandoAceleracao = true; + } } - } - - // ===================================================== - // LEFT TRIGGER - // ===================================================== - - if ( - currentLeftTrigger - - && - - !lastLeftTrigger - ) { - - if (shooterPower == -1.0) { - - shooterPower = - 0.0; + ultimoGatilhoDireito = gatilhoDireitoAtual; + ultimoGatilhoEsquerdo = gatilhoEsquerdoAtual; + + shooter.setPower(shooterActivePower); + spindexer.setPower(shooterActivePower * 0.8); + + // Controle dos Servos com Delay + if (shooterActivePower == 1.0) { // Ativado pelo gatilho direito + servoEsquerdo.setPosition(SERVO_INATIVO); + + if (aguardandoAceleracao && shooterTimer.seconds() >= TEMPO_ACELERACAO) + { + servoDireito.setPosition(SERVO_ATIVO); // Servo direito gira após delay + } + else if (!aguardandoAceleracao) + { + servoDireito.setPosition(SERVO_ATIVO); + } + } else if (shooterActivePower == -1.0) { // Ativado pelo gatilho esquerdo + servoDireito.setPosition(SERVO_INATIVO); + if (aguardandoAceleracao && shooterTimer.seconds() >= TEMPO_ACELERACAO) { + servoEsquerdo.setPosition(SERVO_ATIVO); // Servo esquerdo gira após delay + } else if (!aguardandoAceleracao) { + servoEsquerdo.setPosition(SERVO_ATIVO); + } } else { - - shooterPower = - -1.0; + // Desligado: servos voltam para idle + servoEsquerdo.setPosition(SERVO_INATIVO); + servoDireito.setPosition(SERVO_INATIVO); + aguardandoAceleracao = false; } - } - - // Atualiza estado dos triggers - - lastRightTrigger = - currentRightTrigger; - - lastLeftTrigger = - currentLeftTrigger; - - - // Shooter - - shooter.setPower( - shooterPower - ); - - - // Spindexer - - spindexer.setPower( - shooterPower * 0.8 - ); - } - - // ========================================================= - // FEEDER - // ========================================================= - - private void controlFeeder() { - - if (gamepad2.right_bumper) { - feeder.setPower( - 1.0 - ); + // --- CONTROLE DO FEEDER (Gamepad 2) --- + if (gamepad2.right_bumper) feeder.setPower(1.0); + else if (gamepad2.left_bumper) feeder.setPower(-1.0); + else feeder.setPower(0); - } else if (gamepad2.left_bumper) { - - feeder.setPower( - -1.0 - ); - - } else { - - feeder.setPower( - 0.0 - ); - } - } - - // ========================================================= - // LEITURA DA IMU - // ========================================================= - - private double getHeading() { - - return imu - - .getRobotYawPitchRollAngles() - - .getYaw( - AngleUnit.DEGREES - ); - } - - // ========================================================= - // NORMALIZAÇÃO DO ÂNGULO - // ========================================================= - - private double normalizeAngle( - double angle - ) { - - while (angle > 180.0) { - - angle -= - 360.0; - } - - while (angle < -180.0) { - - angle += - 360.0; - } - - return angle; - } - - // ========================================================= - // DEADZONE - // ========================================================= - - private double applyDeadzone( - double value, - double deadzone - ) { - - if ( - Math.abs(value) - < deadzone - ) { - - return 0.0; - } - - return value; - } - - // ========================================================= - // LIMITADOR - // ========================================================= - - private double clamp( - double value, - double minimum, - double maximum - ) { - - return Math.max( - - minimum, - - Math.min( - maximum, - value - ) - ); - } - - // ========================================================= - // PARA TODOS OS MOTORES - // ========================================================= - - private void stopAllMotors() { - - if (leftFront != null) { - - leftFront.setPower( - 0.0 - ); - } - - if (leftBack != null) { - - leftBack.setPower( - 0.0 - ); - } - - if (rightFront != null) { - - rightFront.setPower( - 0.0 - ); - } - - if (rightBack != null) { - - rightBack.setPower( - 0.0 - ); - } - - if (shooter != null) { - - shooter.setPower( - 0.0 - ); - } - - if (feeder != null) { - - feeder.setPower( - 0.0 - ); - } - - if (spindexer != null) { - - spindexer.setPower( - 0.0 - ); + // Telemetria + telemetry.addData("Shooter", shooterActivePower); + telemetry.addData("Timer", "%.2f", shooterTimer.seconds()); + telemetry.update(); } } } \ No newline at end of file From 3499148bab6bd7c0aa08ed6712476221feb6fa75 Mon Sep 17 00:00:00 2001 From: SESI Mega Snakes FTC Date: Wed, 29 Jul 2026 18:33:14 -0300 Subject: [PATCH 3/4] servos funcionando --- .../firstinspires/ftc/teamcode/Teleop.java | 70 +++++++++++-------- 1 file changed, 40 insertions(+), 30 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java index 9afb514e7ac2..963d6a482a4f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -9,7 +9,7 @@ /** * Código de Teleoperado para a equipe MEGA. * Chassi Mecanum sem Sensores (Sem IMU). - * Inclui Compensação de Strafe (Bias), Shooter, Spindexer, Feeder e Servos com Delay. + * Inclui Compensação de Strafe (Bias), Shooter, Spindexer, Feeder e Servos com Delay de 1s. */ @TeleOp(name = "Teleoperado", group = "TeleOp") public class Teleop extends LinearOpMode { @@ -26,9 +26,9 @@ public class Teleop extends LinearOpMode { // --- CONFIGURAÇÃO DOS SERVOS --- // Posições (0.0 a 1.0). Ajuste conforme a montagem do seu robô. - private double SERVO_INATIVO = 0.0; // Posição inicial (recolhido) - private double SERVO_ATIVO = 0.55; // Posição de 100 graus (aproximadamente 0.55 na maioria dos servos) - private double TEMPO_ACELERACAO = 1.0; // Tempo em segundos para o motor atingir o RPM máximo + private double SERVO_RESET = 0.0; // Posição de RESET (0 graus) + private double SERVO_ATIVO = 0.5; // Posição de 90 graus (aproximadamente 0.5 na maioria dos servos) + private double TEMPO_ESPERA = 1.0; // Tempo em segundos para o motor atingir o RPM máximo @Override public void runOpMode() { @@ -74,9 +74,9 @@ public void runOpMode() { telemetry.addLine("Pronto! (Sem IMU)"); telemetry.update(); - // Inicia servos na posição idle - servoEsquerdo.setPosition(SERVO_INATIVO); - servoDireito.setPosition(SERVO_INATIVO); + // Inicia servos na posição de reset (0) + servoEsquerdo.setPosition(SERVO_RESET); + servoDireito.setPosition(SERVO_RESET); waitForStart(); @@ -116,24 +116,26 @@ public void runOpMode() { boolean gatilhoDireitoAtual = gamepad2.right_trigger > 0.5; boolean gatilhoEsquerdoAtual = gamepad2.left_trigger > 0.5; - // Lógica de Toggle + // Lógica de Toggle RT (Lado Direito / Trás) if (gatilhoDireitoAtual && !ultimoGatilhoDireito) { if (shooterActivePower == -1.0) { shooterActivePower = 0; aguardandoAceleracao = false; } else { shooterActivePower = -1.0; - shooterTimer.reset(); // Inicia contagem para spin-up + shooterTimer.reset(); // Inicia contagem de 1s aguardandoAceleracao = true; } } + + // Lógica de Toggle LT (Lado Esquerdo / Frente) if (gatilhoEsquerdoAtual && !ultimoGatilhoEsquerdo) { if (shooterActivePower == 1.0) { shooterActivePower = 0; aguardandoAceleracao = false; } else { shooterActivePower = 1.0; - shooterTimer.reset(); // Inicia contagem para spin-up + shooterTimer.reset(); // Inicia contagem de 1s aguardandoAceleracao = true; } } @@ -144,29 +146,35 @@ public void runOpMode() { shooter.setPower(shooterActivePower); spindexer.setPower(shooterActivePower * 0.8); - // Controle dos Servos com Delay - if (shooterActivePower == 1.0) { // Ativado pelo gatilho direito - servoEsquerdo.setPosition(SERVO_INATIVO); + // Controle dos Servos com Delay de 1s e Reset para 0 + if (shooterActivePower == 1.0) { // LT LIGADO -> Servo Esquerdo + servoDireito.setPosition(SERVO_RESET); // Garante que o outro está em 0 - if (aguardandoAceleracao && shooterTimer.seconds() >= TEMPO_ACELERACAO) - { - servoDireito.setPosition(SERVO_ATIVO); // Servo direito gira após delay + if (aguardandoAceleracao) { + if (shooterTimer.seconds() >= TEMPO_ESPERA) { + servoEsquerdo.setPosition(SERVO_ATIVO); // Gira para 90 graus após 1s + } else { + servoEsquerdo.setPosition(SERVO_RESET); // Fica em 0 enquanto acelera + } + } else { + servoEsquerdo.setPosition(SERVO_ATIVO); } - else if (!aguardandoAceleracao) - { + } else if (shooterActivePower == -1.0) { // RT LIGADO -> Servo Direito + servoEsquerdo.setPosition(SERVO_RESET); // Garante que o outro está em 0 + + if (aguardandoAceleracao) { + if (shooterTimer.seconds() >= TEMPO_ESPERA) { + servoDireito.setPosition(SERVO_ATIVO); // Gira para 90 graus após 1s + } else { + servoDireito.setPosition(SERVO_RESET); // Fica em 0 enquanto acelera + } + } else { servoDireito.setPosition(SERVO_ATIVO); } - } else if (shooterActivePower == -1.0) { // Ativado pelo gatilho esquerdo - servoDireito.setPosition(SERVO_INATIVO); - if (aguardandoAceleracao && shooterTimer.seconds() >= TEMPO_ACELERACAO) { - servoEsquerdo.setPosition(SERVO_ATIVO); // Servo esquerdo gira após delay - } else if (!aguardandoAceleracao) { - servoEsquerdo.setPosition(SERVO_ATIVO); - } } else { - // Desligado: servos voltam para idle - servoEsquerdo.setPosition(SERVO_INATIVO); - servoDireito.setPosition(SERVO_INATIVO); + // TUDO DESLIGADO -> AMBOS VOLTAM PARA O PONTO ZERO + servoEsquerdo.setPosition(SERVO_RESET); + servoDireito.setPosition(SERVO_RESET); aguardandoAceleracao = false; } @@ -177,8 +185,10 @@ else if (!aguardandoAceleracao) // Telemetria telemetry.addData("Shooter", shooterActivePower); - telemetry.addData("Timer", "%.2f", shooterTimer.seconds()); + telemetry.addData("Timer Servo", "%.2f", shooterTimer.seconds()); + telemetry.addData("Servo Esq", servoEsquerdo.getPosition()); + telemetry.addData("Servo Dir", servoDireito.getPosition()); telemetry.update(); } } -} \ No newline at end of file +} From ac6379b8eb0443d50a9acc0ff3a4365bb6817ed2 Mon Sep 17 00:00:00 2001 From: SESI Mega Snakes FTC Date: Wed, 29 Jul 2026 18:55:39 -0300 Subject: [PATCH 4/4] servos quase funcionando --- .../firstinspires/ftc/teamcode/Teleop.java | 57 ++++++++++--------- 1 file changed, 30 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java index 963d6a482a4f..da93ff72e88a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java @@ -9,7 +9,7 @@ /** * Código de Teleoperado para a equipe MEGA. * Chassi Mecanum sem Sensores (Sem IMU). - * Inclui Compensação de Strafe (Bias), Shooter, Spindexer, Feeder e Servos com Delay de 1s. + * Inclui Compensação de Strafe (Bias), Shooter, Spindexer, Feeder e Servos com Delay de 2.5s. */ @TeleOp(name = "Teleoperado", group = "TeleOp") public class Teleop extends LinearOpMode { @@ -28,7 +28,7 @@ public class Teleop extends LinearOpMode { // Posições (0.0 a 1.0). Ajuste conforme a montagem do seu robô. private double SERVO_RESET = 0.0; // Posição de RESET (0 graus) private double SERVO_ATIVO = 0.5; // Posição de 90 graus (aproximadamente 0.5 na maioria dos servos) - private double TEMPO_ESPERA = 1.0; // Tempo em segundos para o motor atingir o RPM máximo + private double TEMPO_ESPERA = 2.5; // Tempo em segundos para o motor atingir o RPM máximo @Override public void runOpMode() { @@ -67,7 +67,7 @@ public void runOpMode() { boolean ultimoGatilhoEsquerdo = false; double shooterActivePower = 0; - // Timer para o delay dos servos + // Timer para o delay sincronizado ElapsedTime shooterTimer = new ElapsedTime(); boolean aguardandoAceleracao = false; @@ -123,7 +123,7 @@ public void runOpMode() { aguardandoAceleracao = false; } else { shooterActivePower = -1.0; - shooterTimer.reset(); // Inicia contagem de 1s + shooterTimer.reset(); // Inicia contagem de 2.5s aguardandoAceleracao = true; } } @@ -135,7 +135,7 @@ public void runOpMode() { aguardandoAceleracao = false; } else { shooterActivePower = 1.0; - shooterTimer.reset(); // Inicia contagem de 1s + shooterTimer.reset(); // Inicia contagem de 2.5s aguardandoAceleracao = true; } } @@ -143,36 +143,38 @@ public void runOpMode() { ultimoGatilhoDireito = gatilhoDireitoAtual; ultimoGatilhoEsquerdo = gatilhoEsquerdoAtual; + // Liga o Shooter imediatamente shooter.setPower(shooterActivePower); - spindexer.setPower(shooterActivePower * 0.8); - - // Controle dos Servos com Delay de 1s e Reset para 0 - if (shooterActivePower == 1.0) { // LT LIGADO -> Servo Esquerdo - servoDireito.setPosition(SERVO_RESET); // Garante que o outro está em 0 - - if (aguardandoAceleracao) { - if (shooterTimer.seconds() >= TEMPO_ESPERA) { - servoEsquerdo.setPosition(SERVO_ATIVO); // Gira para 90 graus após 1s - } else { - servoEsquerdo.setPosition(SERVO_RESET); // Fica em 0 enquanto acelera - } - } else { - servoEsquerdo.setPosition(SERVO_ATIVO); - } - } else if (shooterActivePower == -1.0) { // RT LIGADO -> Servo Direito - servoEsquerdo.setPosition(SERVO_RESET); // Garante que o outro está em 0 + // --- Lógica Sincronizada (Spindexer + Servos) --- + if (shooterActivePower != 0) { if (aguardandoAceleracao) { if (shooterTimer.seconds() >= TEMPO_ESPERA) { - servoDireito.setPosition(SERVO_ATIVO); // Gira para 90 graus após 1s + // Passou o tempo de espera: liga Spindexer e move o Servo correto + spindexer.setPower(shooterActivePower * 0.8); + + if (shooterActivePower == 1.0) { + servoEsquerdo.setPosition(SERVO_ATIVO); + servoDireito.setPosition(SERVO_RESET); + } else { + servoDireito.setPosition(SERVO_ATIVO); + servoEsquerdo.setPosition(SERVO_RESET); + } } else { - servoDireito.setPosition(SERVO_RESET); // Fica em 0 enquanto acelera + // Ainda acelerando: mantém Spindexer e Servos parados + spindexer.setPower(0); + servoEsquerdo.setPosition(SERVO_RESET); + servoDireito.setPosition(SERVO_RESET); } } else { - servoDireito.setPosition(SERVO_ATIVO); + // Estado de manutenção (se não for o toggle inicial) + spindexer.setPower(shooterActivePower * 0.8); + if (shooterActivePower == 1.0) servoEsquerdo.setPosition(SERVO_ATIVO); + else servoDireito.setPosition(SERVO_ATIVO); } } else { - // TUDO DESLIGADO -> AMBOS VOLTAM PARA O PONTO ZERO + // TUDO DESLIGADO -> RESET IMEDIATO + spindexer.setPower(0); servoEsquerdo.setPosition(SERVO_RESET); servoDireito.setPosition(SERVO_RESET); aguardandoAceleracao = false; @@ -185,7 +187,8 @@ public void runOpMode() { // Telemetria telemetry.addData("Shooter", shooterActivePower); - telemetry.addData("Timer Servo", "%.2f", shooterTimer.seconds()); + telemetry.addData("Spindexer", spindexer.getPower()); + telemetry.addData("Timer", "%.2f", shooterTimer.seconds()); telemetry.addData("Servo Esq", servoEsquerdo.getPosition()); telemetry.addData("Servo Dir", servoDireito.getPosition()); telemetry.update();