ConceptGoBildaStarterKitRobotTeleop_IntoTheDeep.java

java

package org.firstinspires.ftc.teamcode; // пакет проекта FTC (расположение класса в структуре)

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; // класс для линейного (последовательного) режима
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; // аннотация для пользовательского (teleop) режима
import com.qualcomm.robotcore.hardware.CRServo; // серво с непрерывным вращением
import com.qualcomm.robotcore.hardware.DcMotor; // стандартный DC-мотор
import com.qualcomm.robotcore.hardware.DcMotorEx; // расширенная версия мотора с доступом к току и скорости
import com.qualcomm.robotcore.hardware.Servo; // обычное серво
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; // единицы измерения тока (амперы)

/*
 * Этот OpMode — пример телеруправляемого (TeleOp) режима для робота FTC Starter Kit от goBILDA
 * (сезон INTO THE DEEP 2024–2025)
 * Управление выполняется одним оператором с помощью геймпада.
 */
@TeleOp(name="FTC Starter Kit Example Robot (INTO THE DEEP)", group="Robot")
// @Disabled // если раскомментировать, режим не появится в списке на телефоне
public class ConceptGoBildaStarterKitRobotTeleop_IntoTheDeep extends LinearOpMode {

    /* === Объявляем моторы и серво === */
    public DcMotor leftDrive = null;   // левый мотор привода
    public DcMotor rightDrive = null;  // правый мотор привода
    public DcMotor armMotor = null;    // мотор подъёма руки
    public CRServo intake = null;      // серво подачи (захвата), с непрерывным вращением
    public Servo wrist = null;         // обычное серво "запястья"

    /* === Константы и коэффициенты === */
    // Количество энкодерных тиков на один градус поворота руки (с учётом всех передач)
    final double ARM_TICKS_PER_DEGREE =
            28                 // 28 импульсов энкодера на один оборот мотора
            * 250047.0 / 4913.0  // внутреннее редукторное отношение (~50.9:1)
            * 100.0 / 20.0        // внешнее передаточное (шестерни 20:100)
            * 1/360.0;            // делим на 360, чтобы получить "тики на градус"

    /* === Позиции руки (в энкодерных тиках) === */
    final double ARM_COLLAPSED_INTO_ROBOT = 0; // полностью сложено внутрь
    final double ARM_COLLECT = 250 * ARM_TICKS_PER_DEGREE; // позиция для сбора объектов
    final double ARM_CLEAR_BARRIER = 230 * ARM_TICKS_PER_DEGREE; // поднять, чтобы переехать барьер
    final double ARM_SCORE_SPECIMEN = 160 * ARM_TICKS_PER_DEGREE; // позиция для размещения "specimen"
    final double ARM_SCORE_SAMPLE_IN_LOW = 160 * ARM_TICKS_PER_DEGREE; // низкое размещение "sample"
    final double ARM_ATTACH_HANGING_HOOK = 120 * ARM_TICKS_PER_DEGREE; // для зацепа крюком
    final double ARM_WINCH_ROBOT = 15 * ARM_TICKS_PER_DEGREE; // поднять робота (лебёдка)

    /* === Скорости серво подачи (intake) === */
    final double INTAKE_COLLECT = -1.0;  // "всасывание" внутрь
    final double INTAKE_OFF = 0.0;       // выключено
    final double INTAKE_DEPOSIT = 0.5;   // выброс/подача наружу

    /* === Позиции серво запястья (wrist) === */
    final double WRIST_FOLDED_IN = 0.8333; // сложено внутрь
    final double WRIST_FOLDED_OUT = 0.5;   // выдвинуто наружу

    /* === Дополнительные параметры === */
    final double FUDGE_FACTOR = 15 * ARM_TICKS_PER_DEGREE; // шаг коррекции положения руки
    double armPosition = (int)ARM_COLLAPSED_INTO_ROBOT; // начальная позиция руки
    double armPositionFudgeFactor; // переменная для корректировки вручную (триггерами)

    @Override
    public void runOpMode() {

        // переменные для движения робота
        double left;
        double right;
        double forward;
        double rotate;
        double max;

        /* === Инициализация моторов и серво === */
        leftDrive = hardwareMap.get(DcMotor.class, "left_front_drive");
        rightDrive = hardwareMap.get(DcMotor.class, "right_front_drive");
        armMotor = hardwareMap.get(DcMotor.class, "left_arm");

        // направление вращения моторов (чтобы оба ехали вперёд при одинаковом сигнале)
        leftDrive.setDirection(DcMotor.Direction.FORWARD);
        rightDrive.setDirection(DcMotor.Direction.REVERSE);

        // при 0 мощности моторы будут активно тормозить (не катиться)
        leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        // Устанавливаем порог тока для защиты мотора руки (5 ампер)
        ((DcMotorEx) armMotor).setCurrentAlert(5, CurrentUnit.AMPS);

        // Сброс энкодера и установка нулевой позиции
        armMotor.setTargetPosition(0);
        armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        // Инициализация серво
        intake = hardwareMap.get(CRServo.class, "intake");
        wrist = hardwareMap.get(Servo.class, "wrist");

        // Начальные состояния
        intake.setPower(INTAKE_OFF);
        wrist.setPosition(WRIST_FOLDED_IN);

        telemetry.addLine("Robot Ready."); // сообщение на экран оператора
        telemetry.update();

        waitForStart(); // ждём нажатия "СТАРТ"

        /* === Основной цикл управления === */
        while (opModeIsActive()) {

            // Управление движением: вперёд/назад и поворот
            forward = -gamepad1.left_stick_y; // вперёд — отрицательное значение
            rotate = gamepad1.right_stick_x;  // поворот вокруг оси

            // Микширование сигналов
            left = forward + rotate;
            right = forward - rotate;

            // Нормализация значений, чтобы не выйти за диапазон [-1,1]
            max = Math.max(Math.abs(left), Math.abs(right));
            if (max > 1.0) {
                left /= max;
                right /= max;
            }

            // Передача мощности на моторы
            leftDrive.setPower(left);
            rightDrive.setPower(right);

            /* === Управление подачей (intake) === */
            if (gamepad1.a) {
                intake.setPower(INTAKE_COLLECT); // кнопка A — сбор
            } else if (gamepad1.x) {
                intake.setPower(INTAKE_OFF); // X — выключить
            } else if (gamepad1.b) {
                intake.setPower(INTAKE_DEPOSIT); // B — выброс
            }

            /* === Управление положениями руки и запястья === */
            if (gamepad1.right_bumper) {
                armPosition = ARM_COLLECT;
                wrist.setPosition(WRIST_FOLDED_OUT);
                intake.setPower(INTAKE_COLLECT);
            } else if (gamepad1.left_bumper) {
                armPosition = ARM_CLEAR_BARRIER;
            } else if (gamepad1.y) {
                armPosition = ARM_SCORE_SAMPLE_IN_LOW;
            } else if (gamepad1.dpad_left) {
                armPosition = ARM_COLLAPSED_INTO_ROBOT;
                intake.setPower(INTAKE_OFF);
                wrist.setPosition(WRIST_FOLDED_IN);
            } else if (gamepad1.dpad_right) {
                armPosition = ARM_SCORE_SPECIMEN;
                wrist.setPosition(WRIST_FOLDED_IN);
            } else if (gamepad1.dpad_up) {
                armPosition = ARM_ATTACH_HANGING_HOOK;
                intake.setPower(INTAKE_OFF);
                wrist.setPosition(WRIST_FOLDED_IN);
            } else if (gamepad1.dpad_down) {
                armPosition = ARM_WINCH_ROBOT;
                intake.setPower(INTAKE_OFF);
                wrist.setPosition(WRIST_FOLDED_IN);
            }

            /* === Ручная коррекция положения руки триггерами === */
            armPositionFudgeFactor = FUDGE_FACTOR * (gamepad1.right_trigger + (-gamepad1.left_trigger));

            // Применяем скорректированное целевое положение
            armMotor.setTargetPosition((int) (armPosition + armPositionFudgeFactor));
            ((DcMotorEx) armMotor).setVelocity(2100); // скорость вращения (в тиках/сек)
            armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);

            /* === Контроль тока и телеметрия === */
            if (((DcMotorEx) armMotor).isOverCurrent()) {
                telemetry.addLine("ВНИМАНИЕ: мотор руки перегружен по току!");
            }

            telemetry.addData("Целевая позиция руки", armMotor.getTargetPosition());
            telemetry.addData("Текущее положение энкодера", armMotor.getCurrentPosition());
            telemetry.update();
        }
    }
}