4.4.15 FTC – teleoperare – exemplu pentru șasiu cu roti mecanum+ 2 servomotoare

/* Copyright (c) 2017 FIRST. All rights reserved.

 *

 * Redistribution and use in source and binary forms, with or without modification,

 * are permitted (subject to the limitations in the disclaimer below) provided that

 * the following conditions are met:

 *

 * Redistributions of source code must retain the above copyright notice, this list

 * of conditions and the following disclaimer.

 *

 * Redistributions in binary form must reproduce the above copyright notice, this

 * list of conditions and the following disclaimer in the documentation and/or

 * other materials provided with the distribution.

 *

 * Neither the name of FIRST nor the names of its contributors may be used to endorse or

 * promote products derived from this software without specific prior written permission.

 *

 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY’S PATENT RIGHTS ARE GRANTED BY THIS

 * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

 * „AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,

 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE

 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL

 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR

 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,

 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 */

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import com.qualcomm.robotcore.hardware.CRServo;

import com.qualcomm.robotcore.hardware.DcMotor;

import com.qualcomm.robotcore.hardware.DcMotorSimple;

import com.qualcomm.robotcore.hardware.Servo;

import com.qualcomm.robotcore.util.ElapsedTime;

import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;

import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;

import org.firstinspires.ftc.robotcore.external.navigation.Orientation;

/**

 * This file contains an minimal example of a Linear „OpMode”. An OpMode is a ‘program’ that runs in either

 * the autonomous or the teleop period of an FTC match. The names of OpModes appear on the menu

 * of the FTC Driver Station. When an selection is made from the menu, the corresponding OpMode

 * class is instantiated on the Robot Controller and executed.

 *

 * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot

 * It includes all the skeletal structure that all linear OpModes contain.

 *

 * Use Android Studios to Copy this Class, and Paste it into your team’s code folder with a new name.

 * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list

 */

@TeleOp(name=”TeleOpGobilda”, group=”Linear Opmode”)

//@Disabled

public class TeleOperare extends LinearOpMode {

    private ElapsedTime runtime = new ElapsedTime();

    DcMotor leftmotor1;

    DcMotor leftmotor2;

    DcMotor rightmotor1;

    DcMotor rightmotor2;

    Servo servo1;

    Servo servo2;

    @Override

    public void runOpMode() {

        servo1 = hardwareMap.servo.get(„servo1”);

        servo2 = hardwareMap.servo.get(„servo2”);

        leftmotor1 = hardwareMap.dcMotor.get(„stanga fata”);

        leftmotor2 = hardwareMap.dcMotor.get(„stanga spate”);

        rightmotor1 = hardwareMap.dcMotor.get(„dreapta fata”);

        rightmotor2 = hardwareMap.dcMotor.get(„dreapta spate”);

        rightmotor1.setDirection(DcMotorSimple.Direction.REVERSE);

        waitForStart();

        runtime.reset();

        telemetry.addData(„Mode”, „calibrating…”);

        telemetry.update();

        while (opModeIsActive()) {

            telemetry.addData(„pozitie servo1”, servo1.getPosition());

            telemetry.addData(„pozitie servo1”, servo2.getPosition());

            telemetry.update();

            double Y1= -gamepad1.left_stick_y;

            double Y2= -gamepad1.right_stick_y;

            boolean Bumper1= gamepad1.right_bumper;

            boolean Bumper2= gamepad1.left_bumper;

            telemetry.update();

            rightmotor1.setPower(Y1);

            leftmotor1.setPower(Y2);

            rightmotor2.setPower(Y1);

            leftmotor2.setPower(Y2);

            telemetry.update();

            if(Bumper1){

                rightmotor1.setPower(1);

                leftmotor1.setPower(1);

                rightmotor2.setPower(-1);

                leftmotor2.setPower(-1);

            }

            else if (Bumper2){

                rightmotor1.setPower(-1);

                leftmotor1.setPower(-1);

                rightmotor2.setPower(1);

                leftmotor2.setPower(1);

                telemetry.update();

            }

            else {

                rightmotor1.setPower(Y1);

                leftmotor1.setPower(Y2);

                rightmotor2.setPower(Y1);

                leftmotor2.setPower(Y2);

                }

            if(gamepad1.a){servo1.setPosition(0.565);servo2.setPosition(-0.99);telemetry.update();}

            if(gamepad1.b){servo1.setPosition(-0.6);servo2.setPosition(1);telemetry.update();}

            telemetry.update();

        }

    }