Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /* 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.LinearOpMode;
- import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
- import com.qualcomm.robotcore.hardware.DcMotor;
- import com.qualcomm.robotcore.hardware.DcMotorSimple;
- import com.qualcomm.robotcore.util.ElapsedTime;
- import com.qualcomm.robotcore.hardware.Servo;
- import com.qualcomm.robotcore.util.Range;
- @TeleOp(name="OPMode_Principal_Vijai", group="Linear Opmode")
- public class OPMode_Principal extends LinearOpMode {
- // Declaram motoarele si runtime-ul pentru telemetrie
- private ElapsedTime runtime = new ElapsedTime();
- private DcMotor FrontRightMotor = null;
- private DcMotor BackRightMotor = null;
- private DcMotor FrontLeftMotor = null;
- private DcMotor BackLeftMotor = null;
- private DcMotor HookMotor1 = null;
- private DcMotor HookMotor2 = null;
- public Servo Hook = null;
- private static double MAX_POWER = 1.0, MIN_POWER = -1.0, NULL_POWER = 0.0;
- double pozitie;
- private void MotorSetter(double BackLeft, double FrontRight, double FrontLeft, double BackRight){
- BackLeftMotor.setPower(BackLeft);
- FrontRightMotor.setPower(FrontRight);
- FrontLeftMotor.setPower(FrontLeft);
- BackRightMotor.setPower(BackRight);
- }
- @Override
- public void runOpMode() {
- telemetry.addData("Status", "Initialized");
- telemetry.update();
- // Initializam motoare cu nume explicite pentru a putea fi selectate eficient la configurare
- BackLeftMotor = hardwareMap.get(DcMotor.class, "Left_Back");
- FrontRightMotor = hardwareMap.get(DcMotor.class, "Right_Front");
- FrontLeftMotor = hardwareMap.get(DcMotor.class, "Left_Front");
- BackRightMotor = hardwareMap.get(DcMotor.class, "Right_Back");
- HookMotor1 = hardwareMap. get(DcMotor.class, "Hook_Motor_1");
- HookMotor2 = hardwareMap. get(DcMotor.class, "Hook_Motor_2");
- Hook = hardwareMap.get(Servo.class, "Hook_Servo");
- // Datorita regulii maini drepte, inversam motoarele din dreapta si le lasam normale pe cele din dreapta pentru a putea merge drept
- FrontLeftMotor.setDirection(DcMotor.Direction.FORWARD);
- FrontRightMotor.setDirection(DcMotor.Direction.REVERSE);
- BackLeftMotor.setDirection(DcMotor.Direction.FORWARD);
- BackRightMotor.setDirection(DcMotor.Direction.REVERSE);
- HookMotor1.setDirection(DcMotor.Direction.FORWARD);
- HookMotor2.setDirection(DcMotor.Direction.FORWARD);
- // De aici coach-ul apasa start si OpMode-ul va rula pana va apasa stop de pe Driver Station
- Hook.setPosition(pozitie);
- waitForStart();
- runtime.reset();
- if(gamepad2.x)
- Hook.setPosition(0.2);
- else
- Hook.setPosition(0.2);
- // Atata timp cat OpMode-ul este activ va rula pana la oprire urmatorul cod
- while (opModeIsActive()) {
- double Drive, Turn, leftPower ,rightPower;
- //Primirea datelor de la joystick-uri
- Drive = -gamepad1.left_stick_y;
- Turn = gamepad1.right_stick_x;
- //Calcularea puterii redate motoarelor
- leftPower = Range.clip(Drive + Turn, -1.0, 1.0) ;
- rightPower = Range.clip(Drive - Turn, -1.0, 1.0) ;
- //Puterile calculte sunt redate motoarelor
- if (gamepad1.a) {
- BackLeftMotor.setPower(rightPower);
- FrontRightMotor.setPower(leftPower);
- FrontLeftMotor.setPower(leftPower);
- BackRightMotor.setPower(rightPower);
- }
- else{
- BackLeftMotor.setPower(leftPower);
- FrontRightMotor.setPower(rightPower);
- FrontLeftMotor.setPower(leftPower);
- BackRightMotor.setPower(rightPower);
- }
- if(gamepad2.a){
- HookMotor1.setPower(MAX_POWER);
- HookMotor2.setPower(MAX_POWER);
- }
- else{
- HookMotor1.setPower(NULL_POWER);
- HookMotor2.setPower(NULL_POWER);
- }
- if(gamepad2.y){
- HookMotor1.setPower(MIN_POWER);
- HookMotor2.setPower(MIN_POWER);
- }
- else{
- HookMotor1.setPower(NULL_POWER);
- HookMotor2.setPower(NULL_POWER);
- }
- //Setarea puterii pentru diagonale
- if(gamepad1.left_trigger!=0){
- MotorSetter(MAX_POWER, MAX_POWER, NULL_POWER, NULL_POWER);
- }
- if(gamepad1.right_trigger!=0){
- MotorSetter(NULL_POWER, NULL_POWER, MAX_POWER, MAX_POWER );
- }
- if(gamepad1.left_bumper==true){
- MotorSetter(NULL_POWER, NULL_POWER, MIN_POWER, MIN_POWER );
- }
- if(gamepad1.right_bumper==true){
- MotorSetter(MIN_POWER, MIN_POWER, NULL_POWER, NULL_POWER );
- }
- // Afisam pe Driver Station timpul in care robotul a rulat si puterea rotilor
- telemetry.addData("Status", "Run Time: " + runtime.toString());
- telemetry.addData("FrontMotors", "Left (%.2f), Right (%.2f)",leftPower, rightPower);
- telemetry.update();
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement