SHARE
TWEET

Untitled

a guest Nov 17th, 2019 100 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode;
  2.  
  3. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  4. import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  5. import com.qualcomm.robotcore.hardware.DcMotor;
  6. import com.qualcomm.robotcore.hardware.DcMotorSimple;
  7. import com.qualcomm.robotcore.hardware.HardwareMap;
  8. import com.qualcomm.robotcore.util.ElapsedTime;
  9.  
  10. import org.firstinspires.ftc.robotcontroller.external.samples.HardwarePushbot;
  11. import org.firstinspires.ftc.robotcore.external.Telemetry;
  12.  
  13.  
  14. public class Drive {
  15.     /* Declare OpMode members. */
  16.     private ElapsedTime runtime = new ElapsedTime();
  17.  
  18.     static final double COUNTS_PER_MOTOR_REV = 1440;    // eg: TETRIX Motor Encoder
  19.     static final double DRIVE_GEAR_REDUCTION = 2.0;     // This is < 1.0 if geared UP
  20.     static final double WHEEL_DIAMETER_INCHES = 4.0;     // Fornew figuring circumference
  21.     static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
  22.             (WHEEL_DIAMETER_INCHES * 3.1415);
  23.  
  24.     public DcMotor BDL;
  25.     public DcMotor BDR;
  26.     public DcMotor FDR;
  27.     public DcMotor FDL;
  28.     LinearOpMode opMode;
  29.  
  30.     public Drive(LinearOpMode ahwMap) {
  31.         this.opMode = ahwMap;
  32.         BDL = ahwMap.hardwareMap.dcMotor.get("BDL");
  33.         BDR = ahwMap.hardwareMap.dcMotor.get("BDR");
  34.         FDL = ahwMap.hardwareMap.dcMotor.get("FDL");
  35.         FDR = ahwMap.hardwareMap.dcMotor.get("FDR");
  36.  
  37.         BDR.setDirection(DcMotor.Direction.REVERSE);
  38.         BDL.setDirection(DcMotor.Direction.REVERSE);
  39.  
  40.         BDL.setPower(0);
  41.         BDR.setPower(0);
  42.         FDL.setPower(0);
  43.         FDR.setPower(0);
  44.  
  45.         // Set all otors to run without encoders.
  46.         // May want to use RUN_USING_ENCODERS if encoders are installed.
  47.     }
  48.  
  49.     public void encoderDrive(double power,
  50.                              double distance) {
  51.         FDR.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  52.         FDL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  53.         BDR.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  54.         BDL.setMode(DcMotor.RunMode.RUN_TO_POSITION);
  55.  
  56.         int newLeftTarget;
  57.         int newRightTarget;
  58.  
  59.         newLeftTarget = FDR.getCurrentPosition() + (int)(distance * COUNTS_PER_INCH);
  60.         newRightTarget = FDL.getCurrentPosition() + (int)(distance * COUNTS_PER_INCH);
  61.         FDL.setTargetPosition(newLeftTarget);
  62.         FDR.setTargetPosition(newRightTarget);
  63.         BDL.setTargetPosition(newLeftTarget);
  64.         BDR.setTargetPosition(newRightTarget);
  65.  
  66.         FDL.setPower(power);
  67.         BDL.setPower(-power);
  68.         FDR.setPower(power);
  69.         BDR.setPower(power);
  70.  
  71.         //display telemetry data while turning
  72.         while (opMode.opModeIsActive() && (FDL.isBusy() && FDR.isBusy())) {
  73.             // Display it for the driver.
  74.             opMode.telemetry.addData("Path1",  "Running to %7d :%7d", newLeftTarget,  newRightTarget);
  75.             opMode.telemetry.addData("Path2",  "Running at %7d :%7d", FDL.getCurrentPosition(), FDR.getCurrentPosition());
  76.             opMode.telemetry.update();
  77.         }
  78.  
  79.         //stop after done
  80.         FDL.setPower(0);
  81.         BDL.setPower(0);
  82.         FDR.setPower(0);
  83.         BDR.setPower(0);
  84.  
  85.         FDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  86.         FDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  87.         BDR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  88.         BDL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
  89.     }
  90. }
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