public void moveState(double power, double inches, int state, int change) {
    int stateMove = 1;
    telemetry.addData("Move state: ", stateMove);

    switch (stateMove) {
      case 1:
        leftTread.setPower(power);
        rightTread.setPower(power);
        if (Math.abs(leftTread.getCurrentPosition()) >= (inches * TICKS_PER_INCH)) {
          stateMove = 2;
        }
        break;
      case 2:
        leftTread.setPower(0.0);
        rightTread.setPower(0.0);
        stateMove = 3;
        break;
      case 3:
        leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
        rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
        stateMove = 4;
        break;
      case 4:
        if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
          leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
          rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
        }
        state = change;
        break;
      default:
        // End of switch (or bug).
        break;
    }
  }
Esempio n. 2
0
 public int getEncoderAvg() {
   return ((Math.abs(motorBR.getCurrentPosition()))
           + (Math.abs(motorBL.getCurrentPosition()))
           + (Math.abs(motorFR.getCurrentPosition()))
           + (Math.abs(motorFL.getCurrentPosition())))
       / 4;
 }
Esempio n. 3
0
  @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);
      }
    }
  }
  public void turnState(double power, double degrees, int state, int change) {
    int stateTurn = 1;
    telemetry.addData("Turn state: ", stateTurn);

    switch (stateTurn) {
      case 1:
        leftTread.setPower(power);
        rightTread.setPower(-power);
        if (Math.abs(leftTread.getCurrentPosition()) >= (degrees * TICKS_PER_DEGREE)) {
          stateTurn = 2;
        }
        break;
      case 2:
        leftTread.setPower(0.0);
        rightTread.setPower(0.0);
        stateTurn = 3;
        break;
      case 3:
        leftTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
        rightTread.setMode(DcMotor.RunMode.RESET_ENCODERS);
        stateTurn = 4;
        break;
      case 4:
        if ((leftTread.getCurrentPosition() == 0) && (rightTread.getCurrentPosition() == 0)) {
          leftTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
          rightTread.setMode(DcMotor.RunMode.RUN_USING_ENCODERS);
        }
        state = change;
        break;
      default:
        // End of switch (or bug).
        break;
    }
  }
  @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);
        }
    }
  }
  @Override
  public void loop() {
    double rightDrive;
    double leftDrive;

    rightDrive = -gamepad1.right_stick_y;
    leftDrive = -gamepad1.left_stick_y;
    if (gamepad1.dpad_up) {
      rightDrive = 0.3;
      leftDrive = 0.3;
    } else if (gamepad1.dpad_down) {
      rightDrive = -0.3;
      leftDrive = -0.3;
    } else if (gamepad1.dpad_right) {
      rightDrive = -0.3;
      leftDrive = 0.3;
    } else if (gamepad1.dpad_left) {
      rightDrive = 0.3;
      leftDrive = -0.3;
    }
    motorRight.setPower(rightDrive);
    motorLeft.setPower(leftDrive);

    if (gamepad1.a) {
      servoClimberDumper.setPosition(1.0);
    } else {
      servoClimberDumper.setPosition(0.0);
    }

    telemetry.addData("trackLifter", Integer.toString(trackLifter.getCurrentPosition()));
    telemetry.addData("liftCheck", liftCheck.isPressed());
    telemetry.addData("ENCLeft", Integer.toString(motorLeft.getCurrentPosition()));
    telemetry.addData("ENCRight", Integer.toString(motorRight.getCurrentPosition()));
    telemetry.addData("trigger", gamepad1.right_trigger);
  }
Esempio n. 7
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;
  }
  public void getEncoderValues() {
    telemetry.addData("motorBL", motorBL.getCurrentPosition());

    telemetry.addData("motorFR", motorFR.getCurrentPosition());

    telemetry.addData("motorBR", motorBR.getCurrentPosition());

    telemetry.addData("motorFL", motorFL.getCurrentPosition());
  }
Esempio n. 9
0
  protected void printEncoderValues() throws InterruptedException {
    switchToReadOnly();

    telemetry.addData("Motor", "Encoder Position is " + bLeft.getCurrentPosition());
    telemetry.addData("Motor", "Encoder Position is " + bRight.getCurrentPosition());
    telemetry.addData("Motor", "Encoder Position is " + fLeft.getCurrentPosition());
    telemetry.addData("Motor", "Encoder Position is " + fRight.getCurrentPosition());

    switchToWriteOnly();
  }
  public void smoothMoveVol2(double inches, boolean backwards) {
    // works best for at least 1000 ticks = 11.2 inches approx
    double rotations = inches / (Keys.WHEEL_DIAMETER * Math.PI);
    double totalTicks = rotations * 1120 * 3 / 2;
    int positionBeforeMovement = fl.getCurrentPosition();
    double ticksToGo = positionBeforeMovement + totalTicks;
    // p;us one because make the first tick 1, not 0, so fxn will never be 0
    double savedPower = 0;
    double savedTick = 0;
    while (fl.getCurrentPosition() < ticksToGo + 1) {
      telemetry.addData("front left encoder: ", fl.getCurrentPosition());
      telemetry.addData("ticksFor", totalTicks);
      collector.setPower(-1 * Keys.COLLECTOR_POWER);
      // convert to radians
      int currentTick = fl.getCurrentPosition() - positionBeforeMovement + 1;
      if (currentTick < ticksToGo / 2) {
        // use an inv tan function as acceleration
        // power = ((2/pi)*.86) arctan (x/totalticks*.1)
        double power =
            ((2 / Math.PI) * Keys.MAX_SPEED) * Math.atan(currentTick / totalTicks / 2 * 10);
        telemetry.addData("power", "accel" + power);
        if (power < Keys.MIN_SPEED_SMOOTH_MOVE) {
          telemetry.addData("bool", power < Keys.MIN_SPEED_SMOOTH_MOVE);
          power = Keys.MIN_SPEED_SMOOTH_MOVE;
          telemetry.addData("power", "adjusted" + power);
        }
        telemetry.addData("power", power);
        setMotorPowerUniform(power, backwards);
        savedPower = power;
        savedTick = currentTick;
      } else {
        // decelerate using
        double newCurrentCount = currentTick + 1 - savedTick;
        // current tick changes, savedTick is constant
        double horizontalStretch = totalTicks / 2 * .2;
        if (newCurrentCount < horizontalStretch) {
          // becuase of domain restrictions
          setMotorPowerUniform(savedPower, backwards);
        } else {
          // in the domain

          double power =
              (2 / Math.PI) * savedPower * Math.asin(horizontalStretch / newCurrentCount);
          telemetry.addData("power", "decel" + power);
          if (power < Keys.MIN_SPEED_SMOOTH_MOVE) {
            power = Keys.MIN_SPEED_SMOOTH_MOVE;
            telemetry.addData("power", "adjusted" + power);
          }
          setMotorPowerUniform(power, backwards);
        }
      }
    }
    rest();
  }
Esempio n. 11
0
  // 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
  }
  public void moveStraight(double dist, boolean backwards, double power) {

    double rotations = dist / (Keys.WHEEL_DIAMETER * Math.PI);
    double totalTicks = rotations * 1120 * 3 / 2;
    int positionBeforeMovement = fl.getCurrentPosition();
    if (backwards) {
      while (fl.getCurrentPosition() > positionBeforeMovement - totalTicks) {
        setMotorPowerUniform(power, backwards);
      }
    } else {
      while (fl.getCurrentPosition() < positionBeforeMovement + totalTicks) {
        setMotorPowerUniform(power, backwards);
      }
    }
    rest();
  }
Esempio n. 13
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;
  }
Esempio n. 14
0
  @Override
  public void loop() {

    loopctr++;
    motorR();
    // tpos=rightmotor.getTargetPosition();
    // curpos=rightmotor.getCurrentPosition();
    // togo=tpos-curpos;
    telemetry.addData("left mode", leftmotor.getMode());
    telemetry.addData("lf pos", leftmotor.getCurrentPosition());
    telemetry.addData("lf to", leftmotor.getTargetPosition());
    /*if (togo<=0)
    {
       if (runagain==true)
       {
        resetEncode();
        rightPos=800;
        leftPos=800;
        run1();
        runagain=false;
        run3rd=true;
       }
        if (run3rd==true)
        {
            resetEncode();
            rightPos=6625;
            leftPos=-6625;
            run1();
            run3rd=false;
        }
    }*/
  } // end of loop
Esempio n. 15
0
 public void resetEncode() {
   motorW();
   leftmotor.setMode(DcMotorController.RunMode.RESET_ENCODERS);
   rightmotor.setMode(DcMotorController.RunMode.RESET_ENCODERS);
   motorR();
   while (leftmotor.getCurrentPosition() != 0) {}
   return;
 }
Esempio n. 16
0
  public void moveForwardToWallEnc(double pow, int timeout) throws InterruptedException {
    double angle = Math.abs(sensor.getGyroYaw());
    double startAngle = angle;

    double power = pow;

    int tick = 1;

    resetEncoders();

    int startEncoder = Math.abs(motorFL.getCurrentPosition());

    int endEncoder;

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (time.milliseconds() > 250 * tick) {
        endEncoder = Math.abs(motorFL.getCurrentPosition());
        if (startEncoder + 250 < endEncoder) {
          break;
        }
      }

      if (angle < startAngle - 1) {
        startMotors((power * .75), power);
      } else if (angle > startAngle + 1) {
        startMotors(power, (power * .75));
      } else {
        startMotors(power, power);
      }

      opMode.idle();
    }

    stopMotors();

    angleError = sensor.getGyroYaw();
    opMode.telemetry.update();
  }
Esempio n. 17
0
 private double getEncoderAvg() {
   motorBLE = motorBL.getCurrentPosition();
   motorBRE = motorBR.getCurrentPosition();
   motorFLE = motorFL.getCurrentPosition();
   motorFRE = motorFR.getCurrentPosition();
   return (Math.abs(motorBL.getCurrentPosition())
           + Math.abs(motorBR.getCurrentPosition())
           + Math.abs(motorFL.getCurrentPosition())
           + Math.abs(motorFR.getCurrentPosition()))
       / 4;
 }
  public void moveAlteredSin(double dist, boolean backwards) {
    // inches

    double rotations = dist / (Keys.WHEEL_DIAMETER * Math.PI);
    double totalTicks = rotations * 1120 * 3 / 2;
    int positionBeforeMovement = fl.getCurrentPosition();
    while (fl.getCurrentPosition() < positionBeforeMovement + totalTicks) {
      telemetry.addData("front left encoder: ", "sin" + fl.getCurrentPosition());
      telemetry.addData("ticksFor", totalTicks);
      collector.setPower(-1 * Keys.COLLECTOR_POWER);
      // convert to radians
      int currentTick = fl.getCurrentPosition() - positionBeforeMovement;
      // accelerate 15% of time
      // coast 25% of time
      // decelerate 60% of time
      int firstSectionTime = (int) Math.round(.05 * totalTicks);
      int secondSectionTime = (int) (Math.round((.05 + .05) * totalTicks)); // 35
      int thirdSectionTime = (int) (Math.round((.5) * totalTicks)); // 35
      // rest will just be 100%
      double power;
      if (currentTick < firstSectionTime) {

        power = .33;
        // first quarter (period = 2pi) of sin function is only reaching altitude

      } else if (currentTick < secondSectionTime) {
        power = .66;

      } else if (currentTick < thirdSectionTime) {
        power = .86;

      } else {
        // between [40%,100%]
        // decrease time
        int ticksLeft = (int) Math.round(currentTick - (totalTicks * .35));
        // with these ticks left, set a range within cosine to decrease
        power = .4 * Math.cos((ticksLeft) * Math.PI / totalTicks) + .4;
      }

      telemetry.addData("power", power);
      setMotorPowerUniform(power, backwards);
    }
    rest();
  }
Esempio n. 19
0
  // calculates movement updates encoders and looks for buttons pressed
  @Override
  public void loop() {
    telemetry.addData("EncoderAverage", getEncoderAvg());
    telemetry.addData("motorBL", motorBR.getCurrentPosition());
    telemetry.addData("motorFR", motorFR.getCurrentPosition());

    telemetry.addData("motorBR", motorBR.getCurrentPosition());

    telemetry.addData("motorFL", motorFL.getCurrentPosition());
    // controls motion motors
    if (Math.abs(gamepad1.left_stick_y) > .05 || Math.abs(gamepad1.right_stick_y) > .05) {
      startMotors(
          gamepad1.right_stick_y,
          -gamepad1.left_stick_y,
          -gamepad1.left_stick_y,
          gamepad1.right_stick_y);
    } else {
      stopMotors();
    }
    // raises lift
    if (Math.abs(gamepad2.right_trigger) > .05) changeLift(gamepad2.right_trigger);
    else stopLift();
    // lowers lift
    if (Math.abs(gamepad2.left_trigger) > .05) changeLift(-gamepad2.left_trigger);
    else stopLift();
    // resets encoders
    if (gamepad1.a) {
      resetEncoders();
    }
    // sets speed to 1/2
    if (gamepad1.x) {
      setDivider(2);
    }
    // sets speed to 1/4
    if (gamepad1.b) {
      setDivider(4);
    }
    // resets speed to normal
    if (gamepad1.y) {
      setDivider(1);
    }
  }
Esempio n. 20
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);
    }
  }
Esempio n. 21
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);
    }
  }
Esempio n. 22
0
  @Override
  public void runOpMode() {
    elapsedTime = new ElapsedTime();
    dim = hardwareMap.deviceInterfaceModule.get("dim");
    dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
    gyro = hardwareMap.gyroSensor.get("gyro");
    dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
    motorBL = hardwareMap.dcMotor.get("motorBL");
    motorBR = hardwareMap.dcMotor.get("motorBR");
    motorFL = hardwareMap.dcMotor.get("motorFL");
    elapsedTime.startTime();
    motorFR = hardwareMap.dcMotor.get("motorFR");
    //        color = hardwareMap.colorSensor.get("color");
    int distance1 = 5550;
    int distance3 = 9700;
    double currentAngle = 0.0;
    while (motorBL.getCurrentPosition() < distance1) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    while (currentAngle < 90.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }
    while (motorBL.getCurrentPosition() < distance3) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    getTime();
    elapsedTime.reset();
    double currentTime = 0.0;
    while (currentTime < 5.0) {
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    while (motorBL.getCurrentPosition() > 9000) { // 9034
      startMotors(1, -1, -1, 1);
      getEncoderValues();
    }
    while (currentAngle > 45.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }

    elapsedTime.reset();
    currentTime = 0.0;
    while (currentTime < 5.0) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    stopMotors();
    motorBL.close();
    motorFL.close();
    motorBR.close();
    motorFR.close();
  }
Esempio n. 23
0
  protected void move(int targetInches, MoveDirection moveDirection) throws InterruptedException {
    switchToReadOnly();

    double targetPos = (targetInches / INCHES_PER_ROTATION) * NEVEREST_PPR;

    if (moveDirection == MoveDirection.FORWARD) {
      while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
              && Math.abs(fRight.getCurrentPosition()) < targetPos)
          && (Math.abs(bLeft.getCurrentPosition()) < targetPos
              && Math.abs(bRight.getCurrentPosition()) < targetPos)) {

        System.out.println(
            fLeft.getCurrentPosition()
                + " "
                + fRight.getCurrentPosition()
                + " "
                + bLeft.getCurrentPosition()
                + " "
                + bRight.getCurrentPosition());

        System.out.println("Moving" + targetInches + " Inches Forward...");

        switchToWriteOnly();

        fLeft.setPower(SPEED);
        fRight.setPower(SPEED);
        bLeft.setPower(SPEED);
        bRight.setPower(SPEED);

        printEncoderValues();

        switchToReadOnly();
      }
    } else if (moveDirection == MoveDirection.BACKWARD) {
      while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
              && Math.abs(fRight.getCurrentPosition()) < targetPos)
          && (Math.abs(bLeft.getCurrentPosition()) < targetPos
              && Math.abs(bRight.getCurrentPosition()) < targetPos)) {

        System.out.println(
            fLeft.getCurrentPosition()
                + " "
                + fRight.getCurrentPosition()
                + " "
                + bLeft.getCurrentPosition()
                + " "
                + bRight.getCurrentPosition());

        System.out.println("Moving" + targetInches + " Inches Backward...");

        switchToWriteOnly();

        fLeft.setPower(-SPEED);
        fRight.setPower(-SPEED);
        bLeft.setPower(-SPEED);
        bRight.setPower(-SPEED);

        printEncoderValues();

        switchToReadOnly();
      }
    }

    switchToWriteOnly();

    stopMotors();
  }
Esempio n. 24
0
 public boolean aRange(double min, double max) { // Absolute Range (the actual encoder value)
   return (Math.abs((motor.getCurrentPosition() - offset)) < max
       && Math.abs((motor.getCurrentPosition() - offset)) > min);
 }
Esempio n. 25
0
 public double getEnc() {
   return motor.getCurrentPosition();
 }
Esempio n. 26
0
 // resets encoders
 public void resetEncs() {
   enc = 0;
   offset = encOld = motor.getCurrentPosition();
 }
Esempio n. 27
0
 @Override
 public void getValues() {
   enc += motor.getCurrentPosition() - encOld;
 }
Esempio n. 28
0
 @Override
 public void
     calibrate() { // sets the current encoder value as 'old' such that getValues can see the
   // difference
   encOld = motor.getCurrentPosition();
 }
Esempio n. 29
0
  protected void turn(int targetDegrees, TurnDirection turnDirection) throws InterruptedException {
    switchToReadOnly();

    double targetPos = (targetDegrees / DEGREES_PER_ROTATION) * NEVEREST_PPR;

    if (turnDirection == TurnDirection.CLOCKWISE) {
      switchToReadOnly();

      while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
              && Math.abs(fRight.getCurrentPosition()) < targetPos)
          || (Math.abs(bLeft.getCurrentPosition()) < targetPos
              && Math.abs(bRight.getCurrentPosition()) < targetPos)) {

        System.out.println(
            fLeft.getCurrentPosition()
                + " "
                + fRight.getCurrentPosition()
                + " "
                + bLeft.getCurrentPosition()
                + " "
                + bRight.getCurrentPosition());

        System.out.println("Turning " + targetDegrees + " Degrees Clockwise...");

        switchToWriteOnly();

        fLeft.setPower(SPEED);
        fRight.setPower(-SPEED);
        bLeft.setPower(SPEED);
        bRight.setPower(-SPEED);

        switchToReadOnly();
      }

      switchToWriteOnly();
    } else if (turnDirection == TurnDirection.COUNTER_CLOCKWISE) {
      switchToReadOnly();

      while ((Math.abs(fLeft.getCurrentPosition()) < targetPos
              && Math.abs(fRight.getCurrentPosition()) < targetPos)
          || (Math.abs(bLeft.getCurrentPosition()) < targetPos
              && Math.abs(bRight.getCurrentPosition()) < targetPos)) {

        System.out.println(
            fLeft.getCurrentPosition()
                + " "
                + fRight.getCurrentPosition()
                + " "
                + bLeft.getCurrentPosition()
                + " "
                + bRight.getCurrentPosition());

        System.out.println("Turning " + targetDegrees + " Degrees Counter-Clockwise...");

        switchToWriteOnly();

        fLeft.setPower(-SPEED);
        fRight.setPower(SPEED);
        bLeft.setPower(-SPEED);
        bRight.setPower(SPEED);

        switchToReadOnly();
      }

      switchToWriteOnly();
    }

    switchToWriteOnly();

    printEncoderValues();

    stopMotors();
  }
Esempio n. 30
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);
  }