コード例 #1
0
  /**
   * This method does all of the things required to make each side of the robot move forward by the
   * given amounts. Seriously, It changes the channelMode, It calculates the pulse counts
   * Everything! If you understand java and can't use this, you don't deserve to!
   */
  public void measuredDrive(double Rdist, double Ldist) {

    double frrot = Rdist / frontCirc;
    double flrot = Ldist / frontCirc;
    double rPow = .5, lPow = .5;

    leftMotorf.setTargetPosition((int) flrot * pulsePerRot);
    rightMotorf.setTargetPosition((int) frrot * pulsePerRot);

    if (!encodersEnabled) {
      enableEncoders(true);
    }

    if (Rdist < 0) {
      rPow = -rPow;
    } else if (Rdist == 0) {
      rPow = 0;
    }
    if (Ldist < 0) {
      lPow = -lPow;
    } else if (Ldist == 0) {
      lPow = 0;
    }

    leftMotorf.setPower(frontRatio * lPow);
    rightMotorf.setPower(frontRatio * rPow);
  }
コード例 #2
0
  @Override
  public void loop() {
    telemetry.addData("encoders", motor1.getCurrentPosition());
    telemetry.addData("x", x);
    switch (x) {
      case 0:
        motor1.setPower(0.2);
        motor1.setTargetPosition(1120);
        motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

        if (motor1.getCurrentPosition() < 1125 && motor1.getCurrentPosition() > 1115) {
          motor1.setPower(0.0);
          x = 1;
          motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        }
        break;
      case 1:
        motor1.setPower(0.1);
        motor1.setTargetPosition(2400);
        motor1.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

        if (motor1.getCurrentPosition() < 2405 && motor1.getCurrentPosition() > 2395) {
          motor1.setPower(0.0);
          x = 2;
          motor1.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
        }
    }
  }
コード例 #3
0
  public void run1() {
    motorW();
    rnctr++;
    leftmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    rightmotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    telemetry.addData("runct", rnctr);
    while (togo < inchtorun) {
      motorW();
      rightmotor.setTargetPosition(rightPos * togo);
      leftmotor.setTargetPosition(leftPos * togo);
      leftmotor.setPower(.5);
      rightmotor.setPower(.5);
      while (ctr < 50000000) {
        ctr++;
      }

      motorR();
      while (leftmotor.getCurrentPosition() != leftPos) {}
      togo++;
    }
    motorW();
    leftmotor.setPower(0);
    rightmotor.setPower(0);
    // return;
  }
コード例 #4
0
  public void driveDistance(double ldist, double rdist, double power) throws InterruptedException {
    int enc1;
    int enc2;

    enc1 = (int) Math.round(ldist * 33.65);
    enc2 = (int) Math.round(rdist * 33.65);

    leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    leftMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    rightMotor2.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    waitOneFullHardwareCycle();
    leftMotor2.setTargetPosition(enc1);
    rightMotor2.setTargetPosition(enc2);
    leftMotor2.setPower(power);
    rightMotor2.setPower(power);
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    waitOneFullHardwareCycle();
    leftMotorController.setMotorControllerDeviceMode(
        DcMotorController.DeviceMode.READ_ONLY); // Change to read
    rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
    waitOneFullHardwareCycle();
    while (counter == 1) {
      if (isCloseto(leftMotor2.getCurrentPosition(), enc1)
          && isCloseto(rightMotor2.getCurrentPosition(), enc2)) {
        counter = 2;
      }
      waitOneFullHardwareCycle();
    }
    waitOneFullHardwareCycle();
    leftMotorController.setMotorControllerDeviceMode(
        DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
    rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
    waitOneFullHardwareCycle();
    leftMotor2.setPower(0);
    rightMotor2.setPower(0);
    leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    waitOneFullHardwareCycle();
    leftMotorController.setMotorControllerDeviceMode(
        DcMotorController.DeviceMode.READ_ONLY); // Change to read
    rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
    waitOneFullHardwareCycle();
    telemetry.addData("Currentvalue", leftMotor2.getCurrentPosition());
    waitOneFullHardwareCycle();
    leftMotorController.setMotorControllerDeviceMode(
        DcMotorController.DeviceMode.WRITE_ONLY); // Change to read
    rightMotorController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);
    waitOneFullHardwareCycle();
    leftMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    rightMotor2.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    counter = 1;
  }
コード例 #5
0
  public void driveEncoder(double leftPower, double rightPower, double ticks) {
    rightDrive.setTargetPosition(
        (int) ticks); // may need to cast inches to int or long idk also may be in other units
    rightDriveB.setTargetPosition((int) ticks);
    leftDrive.setTargetPosition((int) ticks);
    leftDriveB.setTargetPosition((int) ticks);

    rightDrive.setPower(rightPower);
    rightDriveB.setPower(rightPower);
    leftDrive.setPower(leftPower);
    leftDriveB.setPower(leftPower);
  }
コード例 #6
0
ファイル: TeleOp1.java プロジェクト: zrkdpster/ftc_app
  // code to run the arm
  public void handle_arm() {
    tapeMotor.setPower(0);
    shoulderMotor.setPower(0);
    double shoulder = gamepad2.left_stick_y;
    // double tape = gamepad2.right_stick_y;

    if (shoulder > Positive_Dead_Zone) {
      if ((shoulderMotor.getCurrentPosition() + shoulder_increment) < shoulder_top_limit) {
        // don't go forward past maximum position
        shoulderMotor.setTargetPosition(shoulderMotor.getCurrentPosition() + shoulder_increment);
        shoulderMotor.setPower(shoulderMaxPower);
        telemetry.addData(
            "shoulder : ", "Shoulder up   " + System.out.format("%d", shoulderMotorEncodercurrent));
      } else {
        shoulderMotor.setTargetPosition(shoulder_top_limit);
        shoulderMotor.setPower(shoulderMaxPower);
        telemetry.addData(
            "shoulder : ", "Shoulder up   " + System.out.format("%d", shoulderMotorEncodercurrent));
      }
    } else if (shoulder < Negative_Dead_Zone) {
      if ((shoulderMotor.getCurrentPosition() - shoulder_increment) < shoulder_bottom_limit) {
        // don't go backwards beyond starting position
        shoulderMotor.setTargetPosition(shoulderMotor.getCurrentPosition() - shoulder_increment);
        shoulderMotor.setPower(shoulderMaxPower);
        telemetry.addData(
            "shoulder : ", "Shoulder down " + System.out.format("%d", shoulderMotorEncodercurrent));
      } else {
        shoulderMotor.setTargetPosition(shoulder_bottom_limit);
        shoulderMotor.setPower(shoulderMaxPower);
        telemetry.addData(
            "shoulder : ", "Shoulder down " + System.out.format("%d", shoulderMotorEncodercurrent));
      }
    }

    // need to change below because it's for tape measure motor, which doesn't have encoders

    double tapeValue = -gamepad2.right_stick_y;

    double tapePower = tapeValue * tapeValue;

    if (gamepad2.right_stick_y > Positive_Dead_Zone) {
      // run at scaled power

      tapeMotor.setPower(tapePower * tapeMaxPower);
    } else if (gamepad2.right_stick_y < Negative_Dead_Zone) {

      tapeMotor.setPower(-tapePower * tapeMaxPower);
    }

    // ***************************************8
  }
コード例 #7
0
ファイル: NewTeleOp.java プロジェクト: rollarobotics/team4964
  @Override
  public void loop() {
    telemetry.addData("rightEncoder = ", motorRight.getCurrentPosition());
    if (motorRight.getCurrentPosition() <= -5000) {

      if (finishedForward) {

        finishedForward = false;
        finishedTurning = true;
        telemetry.addData("I did it!", motorRight.getCurrentPosition());

        //   wait(waitValue);

        newValue = motorRight.getCurrentPosition() - 250;

        motorRight.setTargetPosition(-newValue);
        motorRight.setPower(.50);
        motorLeft.setPower(-.50);
      }
    }

    if (motorRight.getCurrentPosition() <= -newValue) {

      if (finishedTurning) {

        finishedTurning = false;

        motorRight.setPower(0);
        motorLeft.setPower(0);
      }
    }
  }
コード例 #8
0
ファイル: CliffHanger.java プロジェクト: IronReign/ftc_app
 public void setCliffElevation() {
   cliffElevation.setTargetPosition((int) (climberTheta * ticksPerDegree));
   if (cliffRelaxDelay < System.nanoTime() && cliffRelaxDelay > 0) {
     cliffRelaxDelay = 0;
     cliffElevation.setPower(0);
   }
 }
コード例 #9
0
  /*
     reverseDriveAuto sets the motors to drive in reverse to the target location
     @param distance     distance to travel in inches
  */
  public void reverseDriveAuto(double distance) {
    double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;

    leftMotor.setTargetPosition((int) counts);
    rightMotor.setTargetPosition((int) counts);

    if (leftMotor.getCurrentPosition() != leftMotor.getTargetPosition()) {

      leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

      reverseDrive(leftMotor, rightMotor, POWER);

    } else {
      leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
      rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    }
  }
コード例 #10
0
ファイル: Autonomous3.java プロジェクト: mvftc/purple_fear
  private void moveDistance(double leftY, double rightY, int distance) throws InterruptedException {
    rightY = -rightY;

    leftbackMotor.setTargetPosition(distance);
    rightbackMotor.setTargetPosition(distance);

    leftbackMotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
    rightbackMotor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);

    leftfrontMotor.setPower(leftY);
    leftbackMotor.setPower(leftY);
    rightfrontMotor.setPower(rightY);
    rightbackMotor.setPower(rightY);

    if (leftbackMotor.getPower() < 0.1) {
      leftfrontMotor.setPower(0.0);
    }
    if (rightbackMotor.getPower() < 0.1) {
      rightfrontMotor.setPower(0.0);
    }
  }
コード例 #11
0
  /*
     forwardDriveAuto method that calculates the distance from inches to encoder counts and then
     sets the motors to drive to the target location
     @param distance     distance to travel in inches
  */
  public void forwardDriveAuto(double distance) {

    double counts = ENCODER_CPR * (distance / (Math.PI * wheelDiameter)) * gearRatio;

    leftMotor.setTargetPosition((int) counts);
    rightMotor.setTargetPosition((int) counts);

    if (leftMotor.getCurrentPosition()
        != leftMotor
            .getTargetPosition()) { // Resets the encoders after the position has been reached

      leftMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
      rightMotor.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

      forwardDrive(leftMotor, rightMotor, POWER);

    } else {
      leftMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
      rightMotor.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    }
  }
コード例 #12
0
ファイル: NewTeleOp.java プロジェクト: rollarobotics/team4964
  @Override
  public void start() {

    motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    //  currentState = 1;

    telemetry.addData("Text", "***Starting***");

    motorRight.setTargetPosition(5000);
    motorRight.setPower(.75);
    //   motorLeft.setTargetPosition(2500);
    motorLeft.setPower(.75);
  }
コード例 #13
0
ファイル: NewTeleOp.java プロジェクト: rollarobotics/team4964
  @Override
  public void init() {
    // currentState = 1;
    knockerRight = hardwareMap.servo.get("knockerRight"); // servo_3
    knockerLeft = hardwareMap.servo.get("knockerLeft"); // servo_2
    backClimber = hardwareMap.servo.get("backClimber"); // servo_4

    // sweeper = hardwareMap.dcMotor.get("motor_5");
    hook = hardwareMap.dcMotor.get("hookMotor"); // motor_3
    winch = hardwareMap.dcMotor.get("winchMotor"); // motor_4
    motorLeft = hardwareMap.dcMotor.get("leftMotor"); // motor_2
    motorRight = hardwareMap.dcMotor.get("rightMotor"); // motor_1

    // physical motors are mounted 180 from each other to both face out to wheels.
    // So, to have robot go forward, one motor needs to be reversed.
    motorLeft.setDirection(DcMotor.Direction.REVERSE);

    // 1/21/16 - adding (motor mode) Reset Encoders before start loop.
    motorRight.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);
    motorLeft.setChannelMode(DcMotorController.RunMode.RESET_ENCODERS);

    telemetry.addData("Text", "***RESETTING***");

    //  motorRight.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);
    //  motorLeft.setChannelMode(DcMotorController.RunMode.RUN_TO_POSITION);

    motorRight.setTargetPosition(0);
    motorRight.setPower(0.0);
    motorLeft.setTargetPosition(0);
    motorLeft.setPower(0.0);

    knockerLeft.setPosition(.52);
    knockerRight.setPosition(.52);
    backClimber.setPosition(.52);
    // REMEMBER: 0.0 to .5 is forward, while .5 to 1.0 is backwards.

  }
コード例 #14
0
ファイル: PushBot4800.java プロジェクト: Team100/ftc_app
  public void loop() {
    arm.setPower(1);
    arm.setTargetPosition(3500);
    if (getRuntime() > 5) {
      revMotor.setPower(0 - 1 * (1.3 / 3));
      if (getRuntime() > 10) launcher.setPosition(0);
    }
    if (getRuntime() > 11) {
      launcher.setPosition(1);
      revMotor.setPower(0);
    }
    if ((getRuntime() > 12) && (getRuntime() < 14)) {
      arm.setTargetPosition(0);
      arm.setPower(-1);
      motorBackLeft.setPower(-1);
      motorFrontLeft.setPower(0.6);
      motorBackRight.setPower(1);
      motorBackRight.setPower(-0.6);
      telemetry.addData("I'm doing it", arm.isBusy());
    }

    telemetry.addData("amIBusy()", arm.isBusy());
    telemetry.update();
  }
コード例 #15
0
ファイル: CliffHanger.java プロジェクト: IronReign/ftc_app
 public void setCliffHangerExtension(double metersOut) {
   cliffHanger1.setTargetPosition(calcCliffHangerTarget(metersOut));
   cliffHanger2.setTargetPosition(cliffHanger1.getTargetPosition());
 }
コード例 #16
0
ファイル: HfDcMotor.java プロジェクト: markdmatthews/ftc_app
 @Override
 public void setTargetPosition(int position) {
   dcMotor.setTargetPosition(position);
 }
コード例 #17
0
  @Override
  public void runOpMode() throws InterruptedException {

    sweeper = hardwareMap.dcMotor.get("motor_4");
    motorLeft = hardwareMap.dcMotor.get("motor_1");
    motorRight = hardwareMap.dcMotor.get("motor_2");
    climbers = hardwareMap.servo.get("servo_1");
    sensorGyro = hardwareMap.gyroSensor.get("gyro");
    // sensorRGB = hardwareMap.colorSensor.get("mr");
    motorLeft.setDirection(DcMotor.Direction.REVERSE);

    wrist = hardwareMap.servo.get("servo_3");
    score = hardwareMap.servo.get("servo_1");
    climbers = hardwareMap.servo.get("servo_2");
    climberknockleft = hardwareMap.servo.get("servo_5");
    climberknockright = hardwareMap.servo.get("servo_6");
    extend = hardwareMap.servo.get("servo_4");
    // button = hardwareMap.servo.get("servo_");

    odsleft = hardwareMap.opticalDistanceSensor.get("odsleft");
    odsright = hardwareMap.opticalDistanceSensor.get("odsright");

    climberknockleft.setPosition(positionclimberknockleftclosed);
    climberknockright.setPosition(positionclimberknockrightclosed);

    arm = hardwareMap.dcMotor.get("motor_3");
    motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
    int Rotation = 1440;
    int Amount = 6;
    int[] Distance = {
      5000, 0, 0, 12000, 8000, 0, 0, 0
    }; // int[] Distance = {2000,2050,5000, 12000, 8000, 0, 0, 0};//int[] Distance =
       // {2000,2050,3000, 12000, 8000, 4000, 0};
    // 4250//11000//10500//11500
    // {2000,2050,5000, 12000, 8000, 0, 0, 0}
    int i = 0;
    int xVal, yVal, zVal = 0;
    int heading = 0;

    // set the starting 2q  S   3qA3eASZzxa saxz of the wrist and neck
    positionwrist = Range.clip(positionwrist, ARM_MIN_RANGE, ARM_MAX_RANGE);
    positionclimber = Range.clip(positionclimber, ARM_MIN_RANGE, ARM_MAX_RANGE);
    positionscore = Range.clip(positionscore, ARM_MIN_RANGE, ARM_MAX_RANGE);

    wrist.setPosition(0.5);
    score.setPosition(0.9);
    climbers.setPosition(0.85);
    Boolean stop = false;
    sensorGyro.calibrate();
    waitForStart();
    while (sensorGyro.isCalibrating()) {
      Thread.sleep(50);
    }

    // write some device information (connection info, name and type)
    // to the log file.

    // get a reference to our GyroSensor object.

    resetStartTime();

    motorLeft.setDirection(DcMotor.Direction.REVERSE);
    motorRight.setDirection(DcMotor.Direction.FORWARD);
    // uncomment this to enable sweeper
    sweeper.setPower(0.85);
    motorRight.setPower(1);
    motorLeft.setPower(0.78);

    boolean reset = false;
    while (opModeIsActive() && this.time < 30) {
      if (i < 7) {

        telemetry.addData("odsleft", odsleft.getLightDetected());
        telemetry.addData("odsright", odsright.getLightDetected());
        telemetry.addData("time: ", this.time);
        telemetry.addData("i=", i);
        if (reset) {
          motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
          sleep(50);
          motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
          sleep(50);
          reset = false;
        }
        if (i == 0) {
          // do nothing because already going right way
        }
        if (i == 1) {
          motorLeft.setDirection(DcMotor.Direction.FORWARD);
          motorRight.setDirection(DcMotor.Direction.REVERSE);
          // change direction to knock down sweeper
        }
        /*if(i==2)
        {
            sweeper.setPower(-0.85);
            motorLeft.setDirection(DcMotor.Direction.REVERSE);
            motorRight.setDirection(DcMotor.Direction.FORWARD);
        }
        if(i==3)
        {sweeper.setPower(-0.85);
            motorLeft.setDirection(DcMotor.Direction.REVERSE);
            motorRight.setDirection(DcMotor.Direction.FORWARD);
            //go parallel to ramp
        }*/
        if (i == 4) {
          sweeper.setPower(-0.85);
          motorLeft.setDirection(DcMotor.Direction.FORWARD);
          motorRight.setDirection(DcMotor.Direction.REVERSE);
          // go to bucket
        }

        if (i == 4) {
          telemetry.addData("here", "here");
          motorLeft.setDirection(DcMotor.Direction.FORWARD);
          motorRight.setDirection(DcMotor.Direction.REVERSE);
          sweeper.setPower(0);
          motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
          motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);

          telemetry.addData("odsleft", odsleft.getLightDetected());
          telemetry.addData("odsright", odsright.getLightDetected());
          if (first) {
            starttime = this.time;
            first = false;
            telemetry.addData("first", "first");
          }
          drop = true;

          condition =
              (odsleft.getLightDetected() < distanceleft
                      || odsright.getLightDetected() < distanceright)
                  && (this.time - starttime < 4.0)
                  && opModeIsActive();
          if (condition) { // only get 3 seconds to line up so motors don't keep going
            /*
             * if (odsleft.getLightDetected() < .5)
             * {
             *
             * */
            telemetry.addData("odsleft: ", odsleft.getLightDetected());
            telemetry.addData("odsright: ", odsright.getLightDetected());
            telemetry.addData("time: ", this.time);
            telemetry.addData("time taken: ", (this.time - starttime));

            if (odsleft.getLightDetected() < distanceleft && opModeIsActive()) {
              left = 0;
              motorLeft.setPower(0.2);
            } else {
              left = 1;
              motorLeft.setPower(0);
            }

            if (odsright.getLightDetected() < distanceright && opModeIsActive()) {
              right = 0;
              motorRight.setPower(0.2);
            } else {
              right = 1;
              motorRight.setPower(0);
            }

          } else {
            motorLeft.setPower(0);
            motorRight.setPower(0);
            i++;
          }
        }

        if (i != 4) {
          motorRight.setTargetPosition(Distance[i]);
          motorLeft.setTargetPosition(Distance[i]);
          motorRight.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
          motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
          motorLeft.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
        }

        if (motorRight.getCurrentPosition() >= Distance[i]
            && motorLeft.getCurrentPosition() >= Distance[i]) {
          motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
          motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
          if (i == 2) {
            sweeper.setPower(0);
            motorLeft.setDirection(DcMotor.Direction.REVERSE);
            motorRight.setDirection(DcMotor.Direction.FORWARD);
            while ((heading < 30 || heading > 40) && opModeIsActive()) { // 323,// 327
              // turn parallel to ramp
              motorLeft.setPower(0.8);
              motorRight.setPower(-1);
              heading = sensorGyro.getHeading();
              telemetry.addData("gyro val: ", heading);
            }
          }
          if (i == 3) {
            sweeper.setPower(0);
            motorLeft.setDirection(DcMotor.Direction.REVERSE);
            motorRight.setDirection(DcMotor.Direction.FORWARD);
            while ((heading < 277 || heading > 279)
                && opModeIsActive()) { // 270,272//329,331//323,// 327
              // turn to dump climbers
              motorLeft.setPower(-0.8);
              motorRight.setPower(1.0);
              heading = sensorGyro.getHeading();
              telemetry.addData("gyro val: ", heading);
            }
            motorLeft.setDirection(DcMotor.Direction.FORWARD);
            motorRight.setDirection(DcMotor.Direction.REVERSE);
          }

          // don't know if this will work
          if (i == 6) {
              /*
              if((servocount>0.02) && opModeIsActive());
              {
                  telemetry.addData("there", "there");
                  telemetry.addData("climber", servocount);
                  climbers.setPosition(servocount);
                  servocount-=0.01;
              }
              */
            if (true || odsright.getLightDetected() > 0.2) {
              climbers.setPosition(0.01);
            }
            i++;
          }
          if (i != 4 && i != 6) {
            drop = false;
          } else {
            drop = true;
          }

          if (!drop) {
            motorRight.setPower(0);
            motorLeft.setPower(0);

            motorRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
            motorLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
            motorRight.setMode(DcMotorController.RunMode.RESET_ENCODERS);
            sleep(50);
            motorLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS);
            sleep(50);
            reset = true;

            motorLeft.setPower(1);
            motorRight.setPower(1);
            sleep(50);
            i++;
          }
        }
      } else {
        if (!end) {
          programtimetaken = this.time;
          end = true;
        }

        motorLeft.setPower(0);
        motorRight.setPower(0);
        telemetry.addData("done", programtimetaken);
        telemetry.addData("odsleft: ", odsleft.getLightDetected());
        telemetry.addData("odsright: ", odsright.getLightDetected());
      }
    }
    motorLeft.setDirection(DcMotor.Direction.FORWARD);
    motorRight.setDirection(DcMotor.Direction.FORWARD);
  }