daily pastebin goal
30%
SHARE
TWEET

Untitled

a guest Feb 23rd, 2019 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. package org.firstinspires.ftc.teamcode.Autonomous;
  2.  
  3. import com.qualcomm.hardware.bosch.BNO055IMU;
  4. import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
  5.  
  6. import org.firstinspires.ftc.teamcode.EasyRobot;
  7.  
  8. @Autonomous(name = "Regional - Timisoara(down)", group = "Official")
  9. public class Mode3 extends EasyRobot {
  10.     @Override
  11.     public void runOpMode() throws InterruptedException {
  12.         initRobot(this);
  13.  
  14.         getCollectorRotateServo().setPosition(0);
  15.  
  16.         initVuforia();
  17.         initTfod(2);
  18.         initGyro(BNO055IMU.AngleUnit.DEGREES);
  19.  
  20. //        lock();
  21.  
  22.         waitForStart();
  23.  
  24.         getCollectorRotateServo().setPosition(1);
  25.  
  26. //        touchDown();
  27.  
  28.         getCollectorRotateServo().setPosition(0.5);
  29.         driveForward(15, 1);
  30.  
  31.         runObjectDetection(2, new ObjectDetected() {
  32.             @Override
  33.             public void pickup() {
  34.                 drive(getHandMotor(), -20, 0.3, true);
  35.                 waitForMotors(getHandMotor().getName());
  36.                 getCollectorRotateServo().setPosition(1);
  37.                 getCollectorServo().setPower(1);
  38.                 drive(getHandMotor(), -20, 0.05, true);
  39.                 waitForMotors(getHandMotor().getName());
  40.                 getCollectorServo().setPower(0);
  41.                 getCollectorRotateServo().setPosition(0.5);
  42.             }
  43.  
  44.             @Override
  45.             public void onLeft() {
  46.                 turnTo(0.3, 0);
  47.             }
  48.  
  49.             @Override
  50.             public void onCenter() {
  51.                 getCollectorRotateServo().setPosition(0.5);
  52.                 sleep(500);
  53.                 driveForward(50, 1);
  54.                 getCollectorRotateServo().setPosition(1);
  55.                 getCollectorServo().setPower(1);
  56.                 driveForward(30, 0.3);
  57.                 waitForMotors("m1", "m2", "m3", "m4");
  58.                 getCollectorRotateServo().setPosition(0.5);
  59.                 getCollectorServo().setPower(0);
  60.                 driveForward(-30, 1);
  61.             }
  62.  
  63.             @Override
  64.             public void onRight() {
  65.                 turnTo(0.3, 0);
  66.             }
  67.  
  68.             @Override
  69.             public void loadCargo() {
  70.                 drive(getHandMotor(), 35, 0.3, true);
  71.                 waitForMotors(getHandMotor().getName());
  72.                 getCollectorRotateServo().setPosition(0);
  73.                 sleep(1500);
  74.                 getCollectorRotateServo().setPosition(0.5);
  75.  
  76.                 driveForward(-13, 1);
  77.                 waitForMotors("m1", "m2", "m3", "m4");
  78. //                driveTicks(getExtendLift(), 1240, 0.5, true, true);
  79. //                waitForMotors(getExtendLift().getName());
  80. //                getExtendLift().setBrake(true);
  81. //
  82. //                getLiftServo1().setPosition(0);
  83. //                getLiftServo2().setPosition(1);
  84. //
  85. //                getExtendLift().setBrake(false);
  86.  
  87.  
  88. //                getLiftServo1().setPosition(0.5);
  89. //                getLiftServo2().setPosition(0.5);
  90.             }
  91.         });
  92.  
  93.         driveForward(50, 0.8);
  94. //        drive(getHandMotor(), -40, 0.5, true);
  95. //        waitForMotors(getHandMotor().getName());
  96. //        getCollectorRotateServo().setPosition(0.5);
  97.  
  98.  
  99.         sleep(5000);
  100.         disable();
  101.     }
  102.  
  103.     private void lock() {
  104.         getExtendLift2().setBrake(true);
  105.         getExtendLift().setBrake(true);
  106.         getCollectorRotateServo().setPosition(0);
  107.     }
  108.  
  109.     private void touchDown() {
  110.         getSortatorServo1().setPosition(1);
  111.         getSortatorServo2().setPosition(1);
  112.         getExtendLift2().setBrake(false);
  113.         getExtendLift().setBrake(false);
  114.  
  115.         driveTicks(getExtendLift(), 1240, 0.1, true, false);
  116.         waitForMotors(getExtendLift().getName());
  117.  
  118.         driveWith(0.3, 15, 15, -15, -15);
  119.         driveForward(10, 0.3);
  120.         driveWith(0.3, -15, -15, 15, 15);
  121.         driveForward(-10, 0.3);
  122.  
  123.         driveTicks(getExtendLift2(), 0, 0.1, true, false);
  124.         waitForMotors(getExtendLift2().getName());
  125.     }
  126.  
  127.     public void driveForward(int distance, double speed) {
  128.         drive(getLeftMotorUp(), -distance, speed, false);
  129.         drive(getRightMotorUp(), distance, speed, false);
  130.         drive(getLeftMotorDown(), -distance, speed, false);
  131.         drive(getRightMotorDown(), distance, speed, false);
  132.         update("m1", "m2", "m3", "m4");
  133.     }
  134.  
  135.     public void driveWith(double speed, int distanceLeftUp, int distanceRightUp, int distanceLeftDown, int distanceRightDown) {
  136.         drive(getLeftMotorUp(), distanceLeftUp, speed, false);
  137.         drive(getRightMotorUp(), distanceRightUp, speed, false);
  138.         drive(getLeftMotorDown(), distanceLeftDown, speed, false);
  139.         drive(getRightMotorDown(), distanceRightDown, speed, false);
  140.         update("m1", "m2", "m3", "m4");
  141.     }
  142.  
  143. }
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