SHARE
TWEET

Untitled

a guest Nov 9th, 2019 69 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3.  
  4.  
  5.  
  6.         import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  7.  
  8.         import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
  9.  
  10.         import com.qualcomm.robotcore.hardware.DcMotor;
  11.  
  12.         import com.qualcomm.robotcore.hardware.DcMotorSimple;
  13.         import com.qualcomm.robotcore.hardware.DigitalChannel;
  14.  
  15.         import com.qualcomm.robotcore.hardware.DistanceSensor;
  16.  
  17.         import com.qualcomm.robotcore.hardware.Gyroscope;
  18.  
  19.         import com.qualcomm.robotcore.hardware.Servo;
  20.  
  21.         import com.qualcomm.robotcore.hardware.configuration.MotorType;
  22.  
  23.  
  24.  
  25. /**
  26.  
  27.  * Created by Boen on 10-10-19
  28.  
  29.  */
  30.  
  31. @TeleOp
  32.  
  33.  
  34.  
  35. public class Mecanum_drive extends LinearOpMode {
  36.  
  37.     private Gyroscope imu;
  38.  
  39.     private DcMotor Back_left_wheel;
  40.  
  41.     private DcMotor Back_right_wheel;
  42.  
  43.     private DcMotor Front_left_wheel;
  44.  
  45.     private DcMotor Front_right_wheel;
  46.  
  47.     private DcMotor Right_linear_lift;
  48.     Servo CLAW;
  49.  
  50.  
  51.  
  52.     @Override
  53.  
  54.     public void runOpMode() {
  55.  
  56.         imu = hardwareMap.get(Gyroscope.class, "imu");
  57.  
  58.         Back_left_wheel = hardwareMap.get(DcMotor.class, "Back_left_wheel");
  59.  
  60.         Back_right_wheel = hardwareMap.get(DcMotor.class, "Back_right_wheel");
  61.  
  62.         Front_left_wheel = hardwareMap.get(DcMotor.class, "Front_left_wheel");
  63.  
  64.         Front_right_wheel = hardwareMap.get(DcMotor.class, "Front_right_wheel");
  65.  
  66.         Right_linear_lift = hardwareMap.get(DcMotor.class, "Right_linear_lift");
  67.         CLAW = hardwareMap.servo.get("CLAW");
  68.  
  69.         Back_left_wheel.setDirection(DcMotor.Direction.REVERSE);
  70.         Back_right_wheel.setDirection(DcMotor.Direction.REVERSE);
  71.         Front_left_wheel.setDirection(DcMotor.Direction.REVERSE);
  72.         Front_right_wheel.setDirection(DcMotor.Direction.REVERSE);
  73.  
  74.         CLAW.getPosition();
  75.  
  76.  
  77.  
  78.  
  79.         telemetry.addData("Status", "Initialized");
  80.  
  81.         telemetry.update();
  82.  
  83.         //Wait for the game to start (driver presses PLAY)
  84.  
  85.         waitForStart();
  86.  
  87.  
  88.  
  89.         //run until the end of the match (driver presses STOP)
  90.  
  91.         double speed;
  92.  
  93.         double rotation;
  94.  
  95.         double strafe;
  96.  
  97.         double motorRotation = 0;
  98.  
  99.  
  100.  
  101.         boolean prevUpButton = false;
  102.  
  103.         boolean prevDownButton = false;
  104.  
  105.  
  106.  
  107.         while (opModeIsActive()) {
  108.  
  109.  
  110.  
  111.             speed = this.gamepad1.left_stick_y;
  112.  
  113.             rotation = -this.gamepad1.left_stick_x;
  114.  
  115.             strafe = -this.gamepad1.right_stick_x;
  116.  
  117.  
  118.  
  119.  
  120.  
  121.             Back_left_wheel.setPower(speed + strafe - rotation);
  122.  
  123.             Back_right_wheel.setPower(-speed + strafe - rotation);
  124.  
  125.             Front_right_wheel.setPower(-speed + strafe + rotation);
  126.  
  127.             Front_left_wheel.setPower(speed + strafe + rotation);
  128.             telemetry.addData("CLAAAAAW position",CLAW.getPosition());
  129.  
  130.  
  131.             telemetry.addData("Target Power", speed);
  132.  
  133.             telemetry.addData("Motor Power", Back_left_wheel.getPower());
  134.  
  135.             telemetry.addData("Status", "Running");
  136.  
  137.             telemetry.update();
  138.  
  139.  
  140.  
  141.             Right_linear_lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  142.  
  143.             // If Up button was just pressed, add 50 to motor rotation
  144.  
  145.             if (gamepad2.right_trigger > 0 && !prevUpButton)
  146.  
  147.             {
  148.  
  149.                 motorRotation += 50;
  150.  
  151.             }
  152.  
  153.             // If Down button was just pressed, subtract 50 to motor rotation
  154.  
  155.             else if (gamepad2.left_trigger > 0 && !prevDownButton)
  156.  
  157.             {
  158.  
  159.                 motorRotation -= 50;
  160.  
  161.             }
  162.  
  163.  
  164.  
  165.             Right_linear_lift.setTargetPosition(motorRotation);
  166.  
  167.             Right_linear_lift.setPower(.5);
  168.  
  169.            double prevUpButton = gamepad2.right_trigger > 0;
  170.  
  171.            double prevDownButton = gamepad2.left_trigger > 0;
  172.  
  173.  
  174.            if (gamepad2.x)
  175.            {
  176.  
  177.                CLAW.setPosition();
  178.  
  179.            }
  180.  
  181.            if (gamepad2.a){
  182.  
  183.                CLAW.setPosition();
  184.            }
  185.         }
  186.  
  187.     }
  188.  
  189. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top