@Override
  public void loop() {

    // Gamepad 1 - Drive Controller

    float rightDriveMotorSpeed = -gamepad1.right_stick_y;
    float leftDriveMotorSpeed = -gamepad1.left_stick_y;

    rightDriveMotorSpeed = Range.clip(rightDriveMotorSpeed, -1, 1);
    leftDriveMotorSpeed = Range.clip(leftDriveMotorSpeed, -1, 1);

    rightDriveMotorSpeed = (float) scaleInput(rightDriveMotorSpeed);
    leftDriveMotorSpeed = (float) scaleInput(leftDriveMotorSpeed);
    System.out.println(rightDriveMotorSpeed);

    driveRight.setPower(rightDriveMotorSpeed);
    driveLeft.setPower(leftDriveMotorSpeed);

    if (gamepad1.a & hookServoClick == 0) {
      hookServoPosition -= hookServoDelta;
      System.out.println(hookServoPosition);
      hookServoClick = 1;
      hookServo.setPosition(hookServoPosition);
    }

    if (gamepad1.a & hookServoClick == 1) {
      hookServoPosition += hookServoDelta;
      System.out.println(hookServoPosition);
      hookServoClick = 0;
      hookServo.setPosition(hookServoPosition);
    }

    if (gamepad1.x & triggerServoClick == 0) {
      triggerServoPosition -= triggerServoDelta;
      System.out.println(triggerServoPosition);
      triggerServoClick = 1;
      triggerServo.setPosition(triggerServoPosition);
    }

    if (gamepad1.x & triggerServoClick == 1) {
      triggerServoPosition += triggerServoDelta;
      System.out.println(triggerServoPosition);
      triggerServoClick = 0;
      triggerServo.setPosition(triggerServoPosition);
    }

    // Gamepad 2

    float armStage1MotorSpeed = -gamepad2.right_stick_y;
    float armStage2MotorSpeed = -gamepad2.left_stick_y;

    armStage1MotorSpeed = Range.clip(armStage1MotorSpeed, -1, 1);
    armStage2MotorSpeed = Range.clip(armStage2MotorSpeed, -1, 1);

    armStage1MotorSpeed = (float) scaleInput(armStage1MotorSpeed);
    armStage2MotorSpeed = (float) scaleInput(armStage2MotorSpeed);

    armStage1.setPower(armStage1MotorSpeed);
    armStage2.setPower(armStage2MotorSpeed);
  }
  public double[] run() {
    final double K_FAST = 1 / (slowDownStart * TICKS_PER_REVOLUTION);
    final double K_SLOW = (1 / slowDownStart - POWER_MIN / fineTuneStart) / TICKS_PER_REVOLUTION;
    final double TICK_OFFSET =
        POWER_MIN
            * slowDownStart
            * fineTuneStart
            * TICKS_PER_REVOLUTION
            / (fineTuneStart - POWER_MIN * slowDownStart);

    double[] currVal = new double[sides];
    double[] error = new double[sides];
    double[] power = new double[sides];

    if (!hasReachedDestination()) {
      for (int i = 0; i < sides; i++) {
        currVal[i] = getCurrentPosition()[i];
        error[i] = targets[i] - currVal[i];
        if (Math.abs(error[i]) < threshold) {
          power[i] = 0.0d;
        } else {
          // try using - fineTuneStart on K_SLOW, maybe need /2
          power[i] =
              (Math.abs(error[i]) > fineTuneStart * TICKS_PER_REVOLUTION)
                  ? K_FAST * Math.abs(error[i])
                  : K_SLOW * (TICK_OFFSET + Math.abs(error[i]));
        }
        if (error[i] < 0.0d) {
          power[i] *= -1;
        }
        power[i] = Range.clip(power[i], -powerMax, powerMax);
      }
    }
    return power;
  }
Exemple #3
0
  double scale_motor_power(double p_power) {
    //
    // Assume no scaling.
    //
    double l_scale = 0.0f;

    //
    // Ensure the values are legal.
    //
    double l_power = Range.clip(p_power, -1, 1);

    double[] l_array = {
      0.00, 0.05, 0.09, 0.10, 0.12, 0.15, 0.18, 0.24, 0.30, 0.36, 0.43, 0.50, 0.60, 0.72, 0.85,
      1.00, 1.00
    };

    //
    // Get the corresponding index for the specified argument/parameter.
    //
    int l_index = (int) (l_power * 16.0);
    if (l_index < 0) {
      l_index = -l_index;
    } else if (l_index > 16) {
      l_index = 16;
    }

    if (l_power < 0) {
      l_scale = -l_array[l_index];
    } else {
      l_scale = l_array[l_index];
    }

    return l_scale;
  }
  /*
   * This method will be called repeatedly in a loop
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
   */
  @Override
  public void loop() {

    /*
     * Gamepad 1
     *
     * Gamepad 1 controls the motors via the left stick, and it controls the
     * wrist/claw via the a,b, x, y buttons
     */

    // throttle: left_stick_y ranges from -1 to 1, where -1 is full up, and
    // 1 is full down
    // direction: left_stick_x ranges from -1 to 1, where -1 is full left
    // and 1 is full right
    float left_throttle = -gamepad1.left_stick_y;
    float right_throttle = -gamepad1.right_stick_y;

    // scale the joystick value to make it easier to control
    // the robot more precisely at slower speeds.
    left_throttle = (float) scaleInput(left_throttle);
    right_throttle = (float) scaleInput(right_throttle);

    // write the values to the motors
    front_right.setPower(right_throttle);
    back_right.setPower(right_throttle);
    front_left.setPower(left_throttle);
    back_left.setPower(left_throttle);

    // update the position of the arm.
    if (gamepad1.y) {
      // if the A button is pushed on gamepad1, increment the position of
      // the arm servo.
      spinnerPosition += spinnerDelta;
    }

    if (gamepad1.a) {
      // if the Y button is pushed on gamepad1, decrease the position of
      // the arm servo.
      spinnerPosition -= spinnerDelta;
    }

    // clip the position values so that they never exceed their allowed range.
    spinnerPosition = Range.clip(spinnerPosition, 0.0, 1.0);

    // write position values to the wrist and claw servo
    spinner.setPosition(spinnerPosition);

    /*
     * Send telemetry data back to driver station. Note that if we are using
     * a legacy NXT-compatible motor controller, then the getPower() method
     * will return a null value. The legacy NXT-compatible motor controllers
     * are currently write only.
     */
    telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("arm", "arm:  " + String.format("%.2f", armPosition));
    telemetry.addData("claw", "claw:  " + String.format("%.2f", clawPosition));
    telemetry.addData("left tgt pwr", "left  pwr: " + String.format("%.2f", left));
    telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
  }
  @Override
  public void loop(RobotContext ctx, LinkedList<Object> out) throws Exception {
    PolarCoordinates joystick = gamepad1().rightJoystick().polar().invert();
    double r = joystick.getR();
    r = Range.clip(2.2842 * Math.pow(Math.tanh(r), 3d), -1, 1);
    joystick = new PolarCoordinates(r, joystick.getTheta());

    // TODO: finish this
  }
  @Override
  public void loop() {
    float lf_power;
    float lb_power;
    float rf_power;
    float rb_power;

    if (gamepad1.left_stick_x > gamepad1.left_stick_y
        || -gamepad1.left_stick_x > -gamepad1.left_stick_y) {
      float left_power = gamepad1.left_stick_x;
      float right_power = gamepad1.left_stick_x;

      left_power = Range.clip(left_power, -1, 1);
      right_power = Range.clip(right_power, -1, 1);

      lf_power = -left_power;
      lb_power = left_power;
      rf_power = right_power;
      rb_power = -right_power;
    } else {
      float left_power = -gamepad1.left_stick_y;
      float right_power = gamepad1.right_stick_y;

      left_power = Range.clip(left_power, -1, 1);
      right_power = Range.clip(right_power, -1, 1);

      lf_power = left_power;
      lb_power = left_power;
      rf_power = right_power;
      rb_power = right_power;
    }

    float turn = (float) scaleInput(gamepad1.right_stick_x);

    lf_power += turn;
    lb_power += turn;
    rf_power += turn;
    rb_power += turn;

    left_front.setPower((float) scaleInput(lf_power));
    left_back.setPower((float) scaleInput(lb_power));
    right_front.setPower((float) scaleInput(rf_power));
    right_back.setPower((float) scaleInput(rb_power));
  }
 /**
  * Moves right climber servo based on {@code enum rightClimberDirection} value set in {@link
  * DriverStation#getNextClimberCmd()}
  *
  * @param drvrcmd {@link DriverCommand} object with values.
  */
 public void applyDSCmd(DriverCommand drvrcmd) {
   if (!rightClimberAvailable) {
     return;
   }
   double rightClimberPosition = rightClimber.getPosition();
   DbgLog.msg(String.format("RightClimber Position = %f", rightClimberPosition));
   switch (drvrcmd.rightClimberCmd.rightClimberStatus) {
     case -1:
       rightClimber.setPosition(Range.clip(0, 0, 1));
       break;
     case 1:
       rightClimber.setPosition(Range.clip(1, 0, 1));
       break;
     case 0:
       break;
     default:
       break;
   }
 }
Exemple #8
0
  public void moveSwiv() {
    if (gamepad1.left_bumper) {
      if (futureSwiv < time) {
        futureSwiv = time + .1;
        leftSwivPos += 0.1;
        leftSwivPos = Range.clip(leftSwivPos, 0, 1);
        rightSwivPos = 1 - leftSwivPos;
      }
    }

    if (gamepad1.right_bumper) {
      if (futureSwiv < time) {
        futureSwiv = time + .1;
        leftSwivPos -= 0.1;
        leftSwivPos = Range.clip(leftSwivPos, 0, 1);
        rightSwivPos = 1 - leftSwivPos;
      }
    }
  }
Exemple #9
0
  public void loop() {
    resetServos();
    moveSwiv();

    swivLeft.setPosition(leftSwivPos);
    swivRight.setPosition((rightSwivPos));
    updateGamepadTelemetry();
    float direction = gamepad1.right_stick_y;
    direction = Range.clip(direction, -1, 1);
    arm.setPosition(scaleContinuous(-gamepad1.right_stick_y));
  }
  @Override
  public void loop() {
    float throttle = -gamepad1.left_stick_y;
    float dirrection = gamepad1.left_stick_x;
    float right = throttle - dirrection;
    float left = throttle + dirrection;

    right = (float) scaleInput(right);
    left = (float) scaleInput(left);

    right = Range.clip(right, -1, 1);
    left = Range.clip(left, -1, 1);

    motorLeft1.setPower(left);
    motorLeft2.setPower(left);
    motorLeft3.setPower(left);

    telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("left tgt pwr", "left  pwr: " + String.format("%.2f", left));
    telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
  }
Exemple #11
0
  @Override
  public void loop() {
    telemetry.addData("!WARNING! ", "!DANGEROUS SERVO TESTING! - " + mode);
    changeMode();
    theDumper.setPosition(Range.clip(lDrivePower, 0.0, 1.0));
    if (!mode) {
      lDrivePower -= 0.01;
    }

    if (mode) {
      lDrivePower += 0.01;
    }
  }
  @Override
  public void loop() {
    Range.clip(motorPower, -1.0d, 1.0d);

    if (gamepad1.right_bumper) {

      motorPower = -gamepad1.right_trigger;
    } else {
      motorPower = gamepad1.right_trigger;
    }

    practiceMotor.setPower(motorPower);
  }
Exemple #13
0
  float ProcessToMotorFromJoy(
      float input) { // This is used in any case where joystick input is to be converted to a motor
    float output = 0.0f;

    // clip the power values so that the values never exceed +/- 1
    output = Range.clip(input, -1, 1);

    // scale the joystick value to make it easier to control
    // the robot more precisely at slower speeds.
    output = (float) scaleInput(output);

    return output;
  }
Exemple #14
0
  public void rotatePB(double pow, int deg) throws InterruptedException {

    double power = pow;
    double angleTo = deg;
    double error;
    double inte = 0;
    double inteNoE = 0;
    double der;

    double currentAngle = sensor.getGyroYaw();
    double previousError = angleTo - currentAngle;

    opMode.telemetry.addData("Current Angle", currentAngle + "");
    opMode.telemetry.addData("Angle To", angleTo + "");
    opMode.telemetry.update();

    opMode.resetStartTime();

    currentAngle = 0;

    while (Math.abs(currentAngle) < Math.abs(angleTo) - 2) {
      currentAngle = sensor.getGyroYaw();
      error = Math.abs(angleTo) - Math.abs(currentAngle);
      opMode.telemetry.addData("error", error);
      power = (pow * (error) * .005) + .1; // update p values
      inte = ((opMode.getRuntime()) * error * .0025); // update inte value
      inteNoE = ((opMode.getRuntime()) * .05);
      der = (error - previousError) / opMode.getRuntime() * 0; // update der value

      power = power + inteNoE + der;

      if (angleTo > 0) power *= -1;

      Range.clip(power, -1, 1);
      startMotors(-power, power);
      opMode.telemetry.addData("PID", power);
      //            opMode.telemetry.addData("integral", inte);
      opMode.telemetry.addData("integral without error", inteNoE);
      opMode.telemetry.addData("angle", currentAngle);

      opMode.telemetry.update();
      previousError = error;
      opMode.idle();
    }

    stopMotors();
    opMode.telemetry.addData("finished", "done");
    opMode.telemetry.update();
  }
  @Override
  public void loop() {
    // get the values from the gamepads
    // note: pushing the stick all the way up returns -1,
    // so we need to reverse the values
    float leftY = -gamepad1.left_stick_y;
    float rightY = -gamepad1.right_stick_y;

    leftY = Range.clip(leftY, -1, 1);
    rightY = Range.clip(rightY, -1, 1);

    // motorLF.setPower(leftY);
    motorLR.setPower(leftY);

    // motorRF.setPower(rightY);
    motorRR.setPower(rightY);

    //   telemetry.addData("left tgt pwr",  "left  pwr: " + String.format("%.2f", left));
    //   telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));

    if (leftY < 1 && rightY > 0) {
      DbgLog.msg("leftY, RightY " + leftY + " " + rightY);
    }
  }
Exemple #16
0
  public void rotatePZeroRev(double pow) throws InterruptedException {

    double power = pow;
    double angleTo = 0;
    double error;
    double inte = 0;
    double inteNoE = 0;
    double der;

    double currentAngle = sensor.getGyroYaw();
    double previousError = angleTo - currentAngle;

    opMode.telemetry.addData("Current Angle", currentAngle + "");
    opMode.telemetry.addData("Angle To", angleTo + "");
    opMode.telemetry.update();

    opMode.resetStartTime();

    while (Math.abs(currentAngle) > 2) {
      currentAngle = sensor.getGyroYaw();
      error = Math.abs(Math.abs(angleTo) - Math.abs(currentAngle));
      opMode.telemetry.addData("error", error);
      power = (pow * (error) * .02) + .125; // update p values
      inte = ((opMode.getRuntime()) * error * .0020); // update inte value
      inteNoE = ((opMode.getRuntime()) * .075);
      der = (error - previousError) / opMode.getRuntime() * 0; // update der value

      power = power + inteNoE - der;

      power *= 1; // -1 is right

      Range.clip(power, -1, 1);
      startMotors(-power, power);
      opMode.telemetry.addData("PID", power);
      //            opMode.telemetry.addData("integral", inte);
      opMode.telemetry.addData("integral without error", inteNoE);
      opMode.telemetry.addData("angle", currentAngle);

      opMode.telemetry.update();
      previousError = error;
      opMode.idle();
    }

    opMode.telemetry.update();
    stopMotors();
  }
  /** Mutate the hand position. */
  void m_hand_position(double p_position) {
    //
    // Ensure the specific value is legal.
    //
    double l_position = Range.clip(p_position, Servo.MIN_POSITION, Servo.MAX_POSITION);

    //
    // Set the value.  The right hand value must be opposite of the left
    // value.
    //
    if (v_servo_left_hand != null) {
      v_servo_left_hand.setPosition(l_position);
    }
    if (v_servo_right_hand != null) {
      v_servo_right_hand.setPosition(1.0 - l_position);
    }
  } // m_hand_position
  /** Scale the joystick input using a nonlinear algorithm. */
  float scale_motor_power(float p_power) {
    //
    // Assume no scaling.
    //
    float l_scale;

    //
    // Ensure the values are legal.
    //
    float l_power = Range.clip(p_power, -1, 1);

    float[] l_array = {
      0.00f, 0.05f, 0.09f, 0.10f, 0.12f,
      0.15f, 0.18f, 0.24f, 0.30f, 0.36f,
      0.43f, 0.50f, 0.60f, 0.72f, 0.85f,
      1.00f, 1.00f
    };

    //
    // Get the corresponding index for the specified argument/parameter.
    //
    int l_index = (int) (l_power * 16.0);
    if (l_index < 0) {
      l_index = -l_index;
    } else if (l_index > 16) {
      l_index = 16;
    }

    if (l_power < 0) {
      l_scale = -l_array[l_index];
    } else {
      l_scale = l_array[l_index];
    }

    return l_scale;
  } // scale_motor_power
Exemple #19
0
  /*
   * This method will be called repeatedly in a loop
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
   */
  @Override
  public void loop() {

    // The op mode should only use "write" methods (setPower, setMode, etc) while in
    // WRITE_ONLY mode or SWITCHING_TO_WRITE_MODE
    if (allowedToWrite()) {
      /*
       * Gamepad 1
       *
       * Gamepad 1 controls the motors via the left stick, and it controls the wrist/claw via the a,b,
       * x, y buttons
       */

      if (gamepad1.dpad_left) {
        // Nxt devices start up in "write" mode by default, so no need to switch modes here.
        motorLeft.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
        motorRight.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS);
      }
      if (gamepad1.dpad_right) {
        // Nxt devices start up in "write" mode by default, so no need to switch modes here.
        motorLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
        motorRight.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
      }

      // throttle:  left_stick_y ranges from -1 to 1, where -1 is full up,  and 1 is full down
      // direction: left_stick_x ranges from -1 to 1, where -1 is full left and 1 is full right
      float throttle = -gamepad1.left_stick_y;
      float direction = gamepad1.left_stick_x;
      float right = throttle - direction;
      float left = throttle + direction;

      // clip the right/left values so that the values never exceed +/- 1
      right = Range.clip(right, -1, 1);
      left = Range.clip(left, -1, 1);

      // write the values to the motors
      motorRight.setPower(right);
      motorLeft.setPower(left);

      // update the position of the wrist
      if (gamepad1.a) {
        wristPosition -= wristDelta;
      }

      if (gamepad1.y) {
        wristPosition += wristDelta;
      }

      // update the position of the claw
      if (gamepad1.x) {
        clawPosition -= clawDelta;
      }

      if (gamepad1.b) {
        clawPosition += clawDelta;
      }

      // clip the position values so that they never exceed 0..1
      wristPosition = Range.clip(wristPosition, 0, 1);
      clawPosition = Range.clip(clawPosition, 0, 1);

      // write position values to the wrist and claw servo
      wrist.setPosition(wristPosition);
      claw.setPosition(clawPosition);

      /*
       * Gamepad 2
       *
       * Gamepad controls the motors via the right trigger as a throttle, left trigger as reverse, and
       * the left stick for direction. This type of control is sometimes referred to as race car mode.
       */

      // we only want to process gamepad2 if someone is using one of it's analog inputs. If you
      // always
      // want to process gamepad2, remove this check
      if (gamepad2.atRest() == false) {

        // throttle is taken directly from the right trigger, the right trigger ranges in values
        // from
        // 0 to 1
        throttle = gamepad2.right_trigger;

        // if the left trigger is pressed, go in reverse
        if (gamepad2.left_trigger != 0.0) {
          throttle = -gamepad2.left_trigger;
        }

        // assign throttle to the left and right motors
        right = throttle;
        left = throttle;

        // now we need to apply steering (direction). The left stick ranges from -1 to 1. If it is
        // negative we want to slow down the left motor. If it is positive we want to slow down the
        // right motor.
        if (gamepad2.left_stick_x < 0) {
          // negative value, stick is pulled to the left
          left = left * (1 + gamepad2.left_stick_x);
        }
        if (gamepad2.left_stick_x > 0) {
          // positive value, stick is pulled to the right
          right = right * (1 - gamepad2.left_stick_x);
        }

        // write the values to the motor. This will over write any values placed while processing
        // gamepad1
        motorRight.setPower(right);
        motorLeft.setPower(left);
      }
    }

    // To read any values from the NXT controllers, we need to switch into READ_ONLY mode.
    // It takes time for the hardware to switch, so you can't switch modes within one loop of the
    // op mode. Every 17th loop, this op mode switches to READ_ONLY mode, and gets the current
    // power.
    if (numOpLoops % 17 == 0) {
      // Note: If you are using the NxtDcMotorController, you need to switch into "read" mode
      // before doing a read, and into "write" mode before doing a write. This is because
      // the NxtDcMotorController is on the I2C interface, and can only do one at a time. If you are
      // using the USBDcMotorController, there is no need to switch, because USB can handle reads
      // and writes without changing modes. The NxtDcMotorControllers start up in "write" mode.
      // This method does nothing on USB devices, but is needed on Nxt devices.
      wheelController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.READ_ONLY);
    }

    // Every 17 loops, switch to read mode so we can read data from the NXT device.
    // Only necessary on NXT devices.
    if (wheelController.getMotorControllerDeviceMode() == DcMotorController.DeviceMode.READ_ONLY) {

      // Update the reads after some loops, when the command has successfully propagated through.
      telemetry.addData("Text", "free flow text");
      telemetry.addData("left motor", motorLeft.getPower());
      telemetry.addData("right motor", motorRight.getPower());
      telemetry.addData("RunMode: ", motorLeft.getMode().toString());

      // Only needed on Nxt devices, but not on USB devices
      wheelController.setMotorControllerDeviceMode(DcMotorController.DeviceMode.WRITE_ONLY);

      // Reset the loop
      numOpLoops = 0;
    }

    // Update the current devMode
    devMode = wheelController.getMotorControllerDeviceMode();
    numOpLoops++;
  }
  /*
   * This method will be called repeatedly in a loop
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
   */
  @Override
  public void loop() {

    /*
     * Gamepad 1
     *
     * Gamepad 1 controls the motors via the left stick, and it controls the
     * wrist/claw via the a,b, x, y buttons
     */

    // throttle: left_stick_y ranges from -1 to 1, where -1 is full up, and
    // 1 is full down
    // direction: left_stick_x ranges from -1 to 1, where -1 is full left
    // and 1 is full right
    float throttle = -gamepad1.left_stick_y;
    float direction = gamepad1.left_stick_x;
    float right = throttle - direction;
    float left = throttle + direction;

    // clip the right/left values so that the values never exceed +/- 1
    right = Range.clip(right, -1, 1);
    left = Range.clip(left, -1, 1);

    // scale the joystick value to make it easier to control
    // the robot more precisely at slower speeds.
    right = (float) scaleInput(right);
    left = (float) scaleInput(left);

    // write the values to the motors
    motorRight.setPower(right);
    motorLeft.setPower(left);
    motorRight2.setPower(right / 3); // front wheels
    motorLeft2.setPower(left / 3); // /3 because of gear ratio
    /*
    /
    /
    /  GAMEPAD 2
    /
    /
    /
    */
    float armThrottle = gamepad2.left_stick_y;
    armThrottle = Range.clip(armThrottle, -1, 1);
    armThrottle = (float) scaleInput(armThrottle);
    // down =  (float)scaleInput(down);
    arm.setPower(armThrottle);
    // arm.setPower(down);

    // update the position of the arm.
    if (gamepad2.a) {
      // if the A button is pushed on gamepad1, increment the position of
      // the arm servo.
      claw1Position += armDelta;
    }

    if (gamepad2.b) {
      // if the Y button is pushed on gamepad1, decrease the position of
      // the arm servo.
      claw1Position -= armDelta;
    }

    // update the position of the claw
    if (gamepad2.a) {
      claw2Position -= clawDelta;
    }

    if (gamepad2.b) {
      claw2Position += clawDelta;
    }

    // clip the position values so that they never exceed their allowed range.
    claw1Position = Range.clip(claw1Position, ARM_MIN_RANGE, ARM_MAX_RANGE);
    claw2Position = Range.clip(claw2Position, CLAW_MIN_RANGE, CLAW_MAX_RANGE);

    // write position values to the wrist and claw servo
    claw1.setPosition(claw1Position);
    claw2.setPosition(claw2Position);

    /*
     * Send telemetry data back to driver station. Note that if we are using
     * a legacy NXT-compatible motor controller, then the getPower() method
     * will return a null value. The legacy NXT-compatible motor controllers
     * are currently write only.
     */

    telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("claw1", "claw1:  " + String.format("%.2f", claw1Position));
    telemetry.addData("claw2", "claw 2:  " + String.format("%.2f", claw2Position));
    telemetry.addData("left tgt pwr", "left  pwr: " + String.format("%.2f", left));
    telemetry.addData("right tgt pwr", "right pwr: " + String.format("%.2f", right));
  }
  @Override
  public void loop() {
    // PullUp!!!
    /*if (gamepad2.left_bumper)
    {
        PullUp_Motors_SetPower(1.0);
    }

    if (gamepad2.left_trigger > 0.2)
    {
        PullUp_Motors_SetPower(-1.0);
    }*/

    // Climber control variable!
    if (!gamepad2.a
        && !gamepad2.b
        && !gamepad2.y
        && gamepad2.right_stick_y > -.05
        && gamepad2.right_stick_y < .05) {
      climber_bypass = false;
    }

    // Pull Up
    if (gamepad2.right_bumper) {
      PullUp_Motors_SetPower(1.0);
    }
    if (gamepad2.right_trigger > 0.2) {
      PullUp_Motors_SetPower(-1.0);
    }

    if (gamepad2.left_stick_y > 0.1) {
      PullUp_Motor_Tape.setPower(-gamepad2.left_stick_y);
    }
    if (gamepad2.left_stick_y < 0.1 && !gamepad2.right_bumper && gamepad2.right_trigger <= 0.2) {
      PullUp_Motor_Tape.setPower(0); // The tape stop.
    }

    if (gamepad2.left_stick_y < -0.1) {
      // PullUp_Motor_String.setPower(-gamepad2.left_stick_y);
    }

    if (gamepad2.left_stick_y > -0.1 && !gamepad2.right_bumper && gamepad2.right_trigger <= 0.2) {
      PullUp_Motor_String.setPower(0); // The string stop.
    }

    if (gamepad2.left_stick_y > -0.1
        && gamepad2.left_stick_y < 0.1
        && !gamepad2.right_bumper
        && gamepad2.right_trigger <= 0.2) {
      PullUp_Motors_SetPower(0); // Both stop.
    }

    // STUFF FOR DRIVE TRAIN!!!
    float left = gamepad1.left_stick_y;
    float right = gamepad1.right_stick_y;

    // clip the right/left values so that the values never exceed +/- 1 (changed range to -.8 to .8
    // to prevent motor overdrive.)
    right = Range.clip(right, -.80F, .80F);
    left = Range.clip(left, -.80F, .80F);

    // write the values to the motors
    setPowerLeftMotor(left);
    setPowerRightMotor(right);

    // CLIMBER!!!
    if (!climber_bypass) {
      Climber_dump(gamepad1.b);
    }

    // FLIPPERS!!!
    if (the_stop_button_left_flipper) {
      if (!gamepad2.dpad_right) {
        the_stop_button_left_flipper = false;
      }
    }

    if (!the_stop_button_left_flipper) {
      if (gamepad2.dpad_right) {
        was_pressed_left_flipper = true;
      }
    }

    if (was_pressed_left_flipper) {
      Flipper_Servo_Left.setPosition(0);

      if (!gamepad2.dpad_right) {
        bypass_left_flipper = true;
      }

      if (bypass_left_flipper) {
        if (gamepad2.dpad_right) {
          was_pressed_left_flipper = false;
          bypass_left_flipper = false;
          the_stop_button_left_flipper = true;
        }
      }
    }

    if (!was_pressed_left_flipper) {
      Flipper_Servo_Left.setPosition(.6); // Was .59
    }

    if (the_stop_button_right_flipper) {
      if (!gamepad2.dpad_left) {
        the_stop_button_right_flipper = false;
      }
    }

    if (!the_stop_button_right_flipper) {
      if (gamepad2.dpad_left) {
        was_pressed_right_flipper = true;
      }
    }

    if (was_pressed_right_flipper) {
      Flipper_Servo_Right.setPosition(1);

      if (!gamepad2.dpad_left) {
        bypass_right_flipper = true;
      }

      if (bypass_right_flipper) {
        if (gamepad2.dpad_left) {
          was_pressed_right_flipper = false;
          bypass_right_flipper = false;
          the_stop_button_right_flipper = true;
        }
      }
    }

    if (!was_pressed_right_flipper) {
      Flipper_Servo_Right.setPosition(.4);
    }

    /*
    if (gamepad2.dpad_left)
    {
        Flipper_Servo_Right.setPosition(1);
    }

    if (!gamepad2.dpad_left)
    {
        Flipper_Servo_Right.setPosition(.57);
    }

    if (gamepad2.dpad_right)
    {
        Flipper_Servo_Left.setPosition(0);
    }

    if (!gamepad2.dpad_right)
    {
        Flipper_Servo_Left.setPosition(.40);
    }
    */

    /*
    if (gamepad2.left_trigger > 0)
    {
        Flipper_Servo_Right.setPosition(1);
    }

    if (gamepad2.left_trigger == 0)
    {
        Flipper_Servo_Right.setPosition(.57);
    }

    if (gamepad2.right_trigger > 0)
    {
        Flipper_Servo_Left.setPosition(0);
    }

    if (gamepad2.right_trigger == 0)
    {
        Flipper_Servo_Left.setPosition(.40);
    }
    */

    // THE CONVEYOR BELT!
    boolean conveyor_left = gamepad1.left_bumper;
    boolean conveyor_right = gamepad1.right_bumper;

    Conveyor_Belt_Control(conveyor_left, conveyor_right);

    // SCOOP!!!
    if (gamepad2.a) {
      Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
      Scoop_Motor.setTargetPosition(5); // Preset for Collecting Debris, previously was 670
      // (assumes scoop is ALL THE WAY TO THE FLOOR AT INIT)
      Scoop_Motor.setPower(.1); // You set this as the max power the motor can have...
      // (foresee issues depending on which side you are on)
      MoveClimberDepositor(0);
    }

    if (gamepad2.b) {
      Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
      Scoop_Motor.setTargetPosition(
          -355); // Preset for Dumping Debris into Conveyor, previously was -375
      Scoop_Motor.setPower(.11);

      MoveClimberDepositor(0);
    }

    if (gamepad2.y) {
      Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
      Scoop_Motor.setTargetPosition(-640); // Preset for Storage, previously was 30
      Scoop_Motor.setPower(.1);

      Climber_servo.setPosition(climber_safe_from_smashing_position);

      climber_bypass = true;
    }

    if (gamepad2.x) {
      int hover_position = Scoop_Motor.getCurrentPosition();
      Scoop_Motor.setMode(DcMotorController.RunMode.RUN_TO_POSITION);
      Scoop_Motor.setTargetPosition(hover_position);
      Scoop_Motor.setPower(.1);
    }

    double Scoop_Motor_value = Range.clip(gamepad2.right_stick_y, -.1, .1);
    double Scoop_Motor_Power =
        Scoop_Motor_value; // Equivalent to x^3? Based on
                           // https://docs.oracle.com/javase/tutorial/java/data/beyondmath.htm

    // if joystick 2, right joystick
    // Stop Motor
    if (gamepad2.right_stick_y < 0.05
        && gamepad2.right_stick_y > -0.05
        && !gamepad2.a
        && !gamepad2.b
        && !gamepad2.y
        && !gamepad2.x) {
      Scoop_Motor.setMode((DcMotorController.RunMode.RUN_USING_ENCODERS));
      Scoop_Motor.setPower(0);
    }

    if (gamepad2.right_stick_y > 0.05 || gamepad2.right_stick_y < -.05) {
      Scoop_Motor.setMode((DcMotorController.RunMode.RUN_USING_ENCODERS));
      Scoop_Motor.setPower(Scoop_Motor_Power);

      MoveClimberDepositor(225);
    }
    // END Scoop Fine-Tuning

    // Sweeper!!!
    boolean locked_controls = false;
    if (gamepad1.a && gamepad1.y) // Pressing A and Y buttons at same time will reset Sweeper.
    {
      locked_controls = true;
      Sweep_servo.setPosition(.5);
    }

    if (the_stop_button_a) {
      if (!gamepad1.a) {
        the_stop_button_a = false;
      }
    }

    if (!the_stop_button_a) {
      if (gamepad1.a) {
        was_pressed_a = true;
      }
    }

    if (was_pressed_a) {
      if (!locked_controls) {
        Sweep_servo.setPosition(.1); // FORWARD SWEEPER!!!
      }

      if (was_pressed_y) {
        was_pressed_y = false;
        bypass_y = false;
      }

      if (!gamepad1.a) {
        bypass_a = true;
      }

      if (bypass_a) {
        if (gamepad1.a) {
          was_pressed_a = false;
          bypass_a = false;
          the_stop_button_a = true;
        }
      }
    }

    if (!was_pressed_a && !was_pressed_y) {
      Sweep_servo.setPosition(.5);
    }

    if (gamepad1.a && gamepad1.y) {
      locked_controls = true;
      Sweep_servo.setPosition(.5);
    }

    if (the_stop_button_y) {
      if (!gamepad1.y) {
        the_stop_button_y = false;
      }
    }

    if (!the_stop_button_y) {
      if (gamepad1.y) {
        was_pressed_y = true;
      }
    }

    if (was_pressed_y) {
      if (!locked_controls) {
        Sweep_servo.setPosition(.9); // REVERSE SWEEPER!!!
      }

      if (was_pressed_a) {
        was_pressed_a = false;
        bypass_a = false;
      }

      if (!gamepad1.y) {
        bypass_y = true;
      }

      if (bypass_y) {
        if (gamepad1.y) {
          was_pressed_y = false;
          bypass_y = false;
          the_stop_button_y = true;
        }
      }
    }

    if (locked_controls) {
      was_pressed_y = false;
      was_pressed_a = false;
      bypass_y = false;
      bypass_a = false;
      the_stop_button_y = false;
      the_stop_button_a = false;
    }

    if (!was_pressed_a && !was_pressed_y) {
      Sweep_servo.setPosition(.5);
    }

    // telemetry.addData("Text", "*** Robot Data***");
    telemetry.addData("Scoop Encoder value", Scoop_Motor.getCurrentPosition());
    telemetry.addData("Scoop Motor Power", Scoop_Motor.getPower());
    telemetry.addData("PullUp Tape Motor Power", PullUp_Motor_Tape.getPower());
    telemetry.addData("PullUp String Motor Power", PullUp_Motor_String.getPower());
    telemetry.addData("Climber Position", Climber_servo.getPosition());
    telemetry.addData("Left Motor Power:" + motorLeft1.getPower(), motorLeft2.getPower());
    telemetry.addData("Right Motor Power:" + motorRight1.getPower(), motorRight2.getPower());
  }
  /** Loop repeatedly called during TeleOp while the robot is running. */
  @Override
  public void loop() {
    // Scale values from analog sticks into range readable by motors
    float leftY = (float) scaleInput(Range.clip(gamepad1.left_stick_y, -1, 1));
    float rightY = (float) scaleInput(Range.clip(gamepad1.right_stick_y, -1, 1));
    float hangMove = -(float) scaleInput(Range.clip(gamepad2.right_stick_y, -1, 1));
    float hangPincher = -(float) scaleInput(Range.clip(gamepad2.left_stick_y, -.5, .5));

    // hangingPower.setPower(hangMove);
    // Control motors for hanging the robot
    if (hangMove > 0.1) hangingPower.setPower(hangMove);
    else if (hangMove < -0.1) hangingPower.setPower(hangMove);
    else hangingPower.setPower(0);

    if (gamepad1.right_bumper) servoPlease.setPosition(.5);
    else if (gamepad1.left_bumper) servoPlease.setPosition(0);

    hangingPincer.setPower(hangPincher);

    if (gamepad2.right_bumper) {
      if (!hangPivotPressed) {
        hangPivotPressed = true;
        if (hangPivot == .75f) hangPivot = 1;
        else if (hangPivot == 1) hangPivot = .75f;
      }
    } else if (hangPivotPressed) hangPivotPressed = false;

    // Control motors for pivoting the hanger
    // Power is based on how far the joystick is moved
    if (gamepad2.dpad_up) {
      hangingPivot.setPower(-hangPivot);
      hangingPivot2.setPower(-hangPivot);
    } else if (gamepad2.dpad_down) {
      hangingPivot.setPower(hangPivot);
      hangingPivot2.setPower(hangPivot);
    } else {
      // Stop the pivot motors
      hangingPivot.setPower(0);
      hangingPivot2.setPower(0);
    }

    // Control servos for controlling the wings
    // *** X Y for left, A B for right
    if (gamepad2.x) LWing.setPosition(0.0);
    else if (gamepad2.y) LWing.setPosition(1.0);
    else LWing.setPosition(0.5);

    if (gamepad2.a) RWing.setPosition(1.0);
    else if (gamepad2.b) RWing.setPosition(0.0);
    else RWing.setPosition(0.5);

    // Reverse the tank drive direction
    if (gamepad1.start) {
      if (!speedReversePressed) {
        speedMultiplier *= -1;
        speedReversePressed = true;
      }
    } else if (speedReversePressed) speedReversePressed = false;

    SetDrivePower(speedMultiplier * rightY, speedMultiplier * leftY);

    telemetry.addData("Drive Right", speedMultiplier * rightY);
    telemetry.addData("Drive Left", speedMultiplier * leftY);
  }
  @Override
  public void loop() {
    // When dpad is pushed up increase one mode
    // When dpad is pushed down decrease by one mode
    if (gamepad1.dpad_up) {
      if (!iSawDpadUpAlready) {
        iSawDpadUpAlready = true;
        mode = mode + 0.25;
      }
    } else {
      iSawDpadUpAlready = false;
    }

    if (gamepad1.dpad_down) {
      if (!iSawDpadDownAlready) {
        iSawDpadDownAlready = true;
        mode = mode - 0.25;
      }
    } else {
      iSawDpadDownAlready = false;
    }
    mode = Range.clip(mode, 0.25, 1);

    // when leftstick is pushed up move forward
    // when rightstick is pushed down move backwards
    double left = -gamepad1.left_stick_y;
    double right = -gamepad1.right_stick_y;

    right = (double) scaleInput(right);
    left = (double) scaleInput(left);

    right = Range.clip(right, -mode, mode);
    left = Range.clip(left, -mode, mode);

    leftFront.setPower(left);
    leftBack.setPower(left);
    rightFront.setPower(right);
    rightBack.setPower(right);

    if (gamepad1.y) {
      vortex.setPower(50);
    } else {
      vortex.setPower(0);
    }

    if (gamepad1.a) {
      vortex.setPower(-50);
    } else {
      vortex.setPower(0);
    }
    if (gamepad2.x) {
      pusherposition = 1;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherLeft.setPower(1);
    } else {
      pusherposition = 0;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherLeft.setPower(0);
    }
    if (gamepad2.b) {
      pusherposition = -1;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherLeft.setPower(-1);
    } else {
      pusherposition = 0;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherLeft.setPower(0);
    }

    if (gamepad2.y) {
      pusherposition = 1;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherRight.setPower(1);
    } else {
      pusherposition = 0;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherRight.setPower(0);
    }
    if (gamepad2.a) {
      pusherposition = -1;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherRight.setPower(-1);
    } else {
      pusherposition = 0;
      pusherposition = Range.clip(pusherposition, -1, 1);
      pusherRight.setPower(0);
    }
  }
  //
  // This method will be called repeatedly in a loop
  // @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
  //
  @Override
  public void loop() {
    String selectedOption = "";

    long startTime = System.currentTimeMillis();

    M_driveRightPower = gamepad1.right_stick_y;
    M_driveLeftPower = gamepad1.left_stick_y;

    if (gamepad1.a && A_upDownServoPos > Servo.MIN_POSITION) A_upDownServoPos -= 0.01d;
    else if (gamepad1.b && A_upDownServoPos < Servo.MAX_POSITION) A_upDownServoPos += 0.01d;

    if (gamepad1.x && A_sideSideServoPos < Servo.MAX_POSITION) A_sideSideServoPos += 0.01d;
    else if (gamepad1.y && A_sideSideServoPos > Servo.MIN_POSITION) A_sideSideServoPos -= 0.01d;

    // setting hardware
    M_driveRight.setPower(M_driveRightPower);
    M_driveLeft.setPower(M_driveLeftPower);

    S_sideSide.setPosition(Range.clip(A_sideSideServoPos, 0.0, 1.0));
    S_upDown.setPosition(Range.clip(A_upDownServoPos, 0.0, 1.0));

    // Employ the enum option set in init_loop with if and switch statements.

    if (choiceValue == Options.Option_1) selectedOption = "Option 1";

    switch (choiceValue) {
      case Option_2:
        selectedOption = "Option 2";
        break;

      case Option_3:
        selectedOption = "Option 3";
        break;
    }

    // display telemetry on DS. line labels are sorted.
    Util.telemetry(
        "1-LeftGP",
        "LY=%.2f LP=%.2f  RY=%.2f RP=%.2f",
        gamepad1.left_stick_y,
        M_driveLeftPower,
        gamepad1.right_stick_y,
        M_driveRightPower);
    Util.telemetry(
        "Buttons",
        "t=%b x=%b a=%b b=%b y=%b",
        I_touch.isPressed(),
        gamepad1.x,
        gamepad1.a,
        gamepad1.b,
        gamepad1.y);
    Util.telemetry("UpDwn Servo", "Sup=%.2f real=%.2f", A_upDownServoPos, S_upDown.getPosition());
    Util.telemetry(
        "Sidew Servo", "Sup=%.2f real=%.2f", A_sideSideServoPos, S_sideSide.getPosition());
    //        Util.telemetry("Options", "String=%s   Enum=%s  SelEnum=%s", option,
    // enumOption.toString(), selectedEnumOption);

    Util.telemetry("Menu selection", "%d=%s (%s)", choice, choiceValue.name(), selectedOption);

    Util.telemetry("Outside Loop Time", Long.toString(startTime - lastLoopTime));

    long endTime = System.currentTimeMillis();
    Util.telemetry("Loop Time", Long.toString(endTime - startTime));

    lastLoopTime = endTime;
  }
  @Override
  public void runOpMode() throws InterruptedException {

    // initMediaPlayer();

    leftMotor = hardwareMap.dcMotor.get("leftdrive");
    rightMotor = hardwareMap.dcMotor.get("rightdrive");
    armmotor1 = hardwareMap.dcMotor.get("armmotor1");
    armmotor2 = hardwareMap.dcMotor.get("armmotor2");
    spinner = hardwareMap.dcMotor.get("spinner");
    turner = hardwareMap.dcMotor.get("turner");
    zipLine1 = hardwareMap.servo.get("servo1");
    zipLine2 = hardwareMap.servo.get("servo2");
    leftMotor.setDirection(DcMotor.Direction.REVERSE);
    armOn = false;
    waitForStart();

    // assign the starting position of the servos
    zipPosition1 = 1.0;
    zipPosition2 = 0;

    while (opModeIsActive()) {

      // Change Direction
      if (gamepad1.right_bumper) {
        if (reverseOn) {
          reverseOn = false;
        } else if (!reverseOn) {
          reverseOn = true;
        }
      }

      float throttle_r = -gamepad1.right_stick_y;
      float throttle_l = -gamepad1.left_stick_y;
      float right = throttle_r;
      float left = throttle_l;

      // This is for sniper mode
      if (gamepad1.a) {
        if (sniperOn) {
          sniperOn = false;
        } else {
          sniperOn = true;
        }
      }
      if (!sniperOn) {
        // clip the right/left values so that the values never exceed +/- 1
        right = Range.clip(right, -1, 1);
        left = Range.clip(left, -1, 1);

        // write the values to the motors
        rightMotor.setPower(right);
        leftMotor.setPower(left);
      } else if (sniperOn) {
        // clip the right/left values so that the values never exceed +/- .5
        right = Range.clip(right, -.5f, .5f);
        left = Range.clip(left, -.5f, .5f);

        // write the values to the motors
        rightMotor.setPower(right);
        leftMotor.setPower(left);
      }

      if (gamepad2.dpad_down) {
        // move the servos to hit the ziplines
        zipPosition1 = 0.2D;
        zipPosition2 = 0.7D;
      }

      if (gamepad2.dpad_up) {
        zipPosition1 = 1.0;
        zipPosition2 = 0;
      }

      if (gamepad2.y) {
        spinner.setPower(-0.9);
      } else if (gamepad2.x) {
        spinner.setPower(0.9);
      } else {
        if (!gamepad2.y && !gamepad2.x) {
          spinner.setPower(0);
        }
      }
      if (gamepad2.right_bumper) {
        turner.setPower(-0.5);
      } else if (gamepad2.left_bumper) {
        turner.setPower(0.5);
      } else {
        if (!gamepad2.right_bumper && !gamepad2.left_bumper) {
          turner.setPower(0);
        }
      }

      if (gamepad2.a) {
        armmotor1.setPower(-0.2);
        armmotor2.setPower(0.2);
        armOn = true;
      } else if (gamepad2.b) {
        armmotor1.setPower(0.2);
        armmotor2.setPower(-0.2);
        armOn = true;
      } else {
        if (armOn) {
          armmotor1.setPower(0);
          armmotor2.setPower(0);
          armOn = false;
        }
      }

      // if (gamepad2.a){
      //    spinnerMotor.setPower(1);
      // }
      // if (gamepad2.x){
      //    spinnerMotor.setPower(-1);
      // }
      // if (!gamepad2.a && !gamepad2.x){
      //    spinnerMotor.setPowerFloat();
      // }

      // write position values to the wrist and claw servo
      zipLine1.setPosition(zipPosition1);
      zipLine2.setPosition(zipPosition2);

      // if (gamepad1.left_bumper) {
      //    toggleMediaPlayer();
      // }

      /*
       * Send telemetry data back to driver station.
       */
      telemetry.addData("Text", "*** TestBot Data***");
      telemetry.addData("left tgt pwr", "left  pwr: " + String.format("%.2f", left));
      telemetry.addData("tgt pwr", "right pwr: " + String.format("%.2f", right));

      waitOneFullHardwareCycle();

      // stopMediaPlayer();
    }
  }
  /*
   * This method will be called repeatedly in a loop
   *
   * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#run()
   */
  @Override
  public void loop() {

    double motor_1_stick;
    double motor_2_stick;

    // ON CONTROLLER:
    // - no light next to mode button
    // - switch set to x on back
    // IN DRIVER APP
    // - controller set as Logitech F310 Gamepad
    //
    // y value: -1 = up, +1 = down
    // x value: -1 = left, +1 = right
    //

    // Joystick y inverted, so then the motor speed is
    // positive when pushing stick up
    double motor_speed = -gamepad1.left_stick_y;

    // Just get the plain stick x and use as the turn speed
    double turn_speed = gamepad1.right_stick_x;

    // Joystick is pushed forward or backward
    if (motor_speed != 0) {

      // If the turn speed is maxed at either 1 or -1,
      // this variable will cancel out the motor speed (by subtraction).
      // This is useful in turning (while moving) by preventing
      // motor from going the opposite direction and making things go crazy.
      double turn_speed_to_halt = abs_d(turn_speed) * motor_speed;

      // If joystick is push to the left (turn_speed < 0),
      // then motor 2 will slow down.
      motor_2_stick = motor_speed - ((turn_speed < 0) ? turn_speed_to_halt : 0);

      // If joystick is push to the right (turn_speed > 0),
      // then motor 1 will slow down.
      motor_1_stick = motor_speed - ((turn_speed > 0) ? turn_speed_to_halt : 0);
    }

    // Basic 'standing' turn
    else {
      motor_1_stick = -turn_speed;
      motor_2_stick = turn_speed;
    }

    // clip values so to be in [-1,1]
    motor_1_stick = Range.clip(motor_1_stick, -1, 1);
    motor_2_stick = Range.clip(motor_2_stick, -1, 1);
    // call motor response function
    motor_1_stick = (float) response_function(motor_1_stick);
    motor_2_stick = (float) response_function(motor_2_stick);

    // set motor values (inverted because of motor placement)
    motor_1.setPower(-motor_1_stick);
    motor_2.setPower(-motor_2_stick);

    if (gamepad1.a && !touch_sensor.isPressed()) {
      motor_3.setPower(motor_3_delta);
    } else if (gamepad1.y) {
      motor_3.setPower(-motor_3_delta);
    } else {
      motor_3.setPower(0);
    }

    // Left dpad, move servo, then move back after 3 sec
    if (gamepad1.dpad_left) {
      servo1Timer.setWaitPeriod(3000);
      // servo_1_position = SERVO_MAX;
      servo_1.setPosition(SERVO_MAX);
    }
    if (servo1Timer.isDone()) {
      servo_1.setPosition(SERVO_MIN);
    }

    // Up dpad, move servo, then move back after 3 sec
    if (gamepad1.dpad_up) {
      servo2Timer.setWaitPeriod(3000);
      servo_2.setPosition(SERVO_MAX);
    }
    if (servo2Timer.isDone()) {
      servo_2.setPosition(SERVO_MIN);
    }

    // Right dpad, move servo, then move back after 3 sec

    if (gamepad1.dpad_right) {
      servo3Timer.setWaitPeriod(3000);
      servo_3.setPosition(SERVO_MAX);
    }
    if (servo3Timer.isDone()) {
      servo_3.setPosition(SERVO_MIN);
    }

    // use a to ratchet down servo 2, x to ratchet down servo 1, y to ratchet up servo 2 and b to
    // ratchet up servo 1
    /*if(gamepad1.left_bumper){
    	servo_1_position-=servo_1_delta;
    }
    if(gamepad1.left_trigger>0){
    	servo_1_position+=servo_1_delta;
    }*/

    // clip servos to prespectified range
    servo_1_position = Range.clip(servo_1_position, SERVO_MIN, SERVO_MAX);

    // set servo positions
    servo_1.setPosition(servo_1_position);

    /*if(touch_sensor.isPressed()) {
    	motor_1.setPower(0.00);
    	motor_2.setPower(0.00);
    	motor_1_stick = 0.00;
    	motor_2_stick = 0.00;
    }*/

    /*double ir_angle = 0.0;
    double ir_strength = 0.0;
    if (ir_seeker.signalDetected()) {
    	/*
    	 * Get angle and strength of the signal.
    	 * Note an angle of zero implies straight ahead.
    	 * A negative angle implies that the source is to the left.
    	 * A positive angle implies that the source is to the right.
    	 /
    	ir_angle = ir_seeker.getAngle();
    	ir_strength = ir_seeker.getStrength();
    }*/

    // write some telemetry to the driver station
    //	telemetry.addData("Text","*** Robot Data ***");
    //	telemetry.addData("servo_1","servo_1: "+String.format("%.2f",servo_1_position));
    //	telemetry.addData("servo_2","servo_2: "+String.format("%.2f",servo_2_position));
    telemetry.addData("motor_1", "motor 1: " + String.format("%.2f", motor_1_stick));
    telemetry.addData("motor_2", "motor 2: " + String.format("%.2f", motor_2_stick));
    telemetry.addData("turn_speed", "turn_speed: " + String.format("%.2f", turn_speed));
    telemetry.addData("motor_speed", "motor_speed: " + String.format("%.2f", motor_speed));
    //	telemetry.addData("ir detected","ir detected:
    // "+String.format("%d",ir_seeker.signalDetected()));
    //	telemetry.addData("ir angle","ir angle: "+String.format("%.2f",ir_angle));
    //	telemetry.addData("ir strength","ir strength: "+String.format("%.2f",ir_strength));
    //	telemetry.addData("ods value","ods value: "+String.format("%.2f",light_detected));

  }
Exemple #27
0
  void handle_drivetrain() {

    if (arcademode) { //  code for arcade mode here

      telemetry.addData("Drive: ", drive_mode(arcademode));

      double xValue = -gamepad1.left_stick_x;
      double yValue = -gamepad1.left_stick_y;

      xValue = xValue * maxXJoystick;

      double xSquaredVal = xValue * xValue;
      double ySquaredVal = yValue * yValue;

      double xPower;
      double yPower;

      if (xValue < 0) {
        xPower = -xSquaredVal;
      } else {
        xPower = xSquaredVal;
      }

      if (yValue < 0) {
        yPower = -ySquaredVal;
      } else {
        yPower = ySquaredVal;
      }

      double leftPower = yPower + xPower;
      double rightPower = yPower - xPower;

      leftPower = Range.clip(leftPower, -1, 1);
      rightPower = Range.clip(rightPower, -1, 1);

      leftMotor.setPower(leftPower * arcadeMaxPower);

      rightMotor.setPower(rightPower * arcadeMaxPower);

    } else { //  code for tank mode here

      telemetry.addData("Drive: ", drive_mode(arcademode));

      double leftY = -gamepad1.left_stick_y;
      double rightY = -gamepad1.right_stick_y;

      double leftSquaredVal = leftY * leftY;
      double rightSquaredVal = rightY * rightY;

      double leftPower;
      double rightPower;

      if (leftY < 0) {
        leftPower = -leftSquaredVal;
      } else {
        leftPower = leftSquaredVal;
      }

      if (rightY < 0) {
        rightPower = -rightSquaredVal;
      } else {
        rightPower = rightSquaredVal;
      }

      leftMotor.setPower(leftPower * tankMaxPower);

      rightMotor.setPower(rightPower * tankMaxPower);
    }
  }
 void gripperPosition(double p_position) {
   double l_position = Range.clip(p_position, Servo.MIN_POSITION + 0.1, Servo.MAX_POSITION - 0.1);
   gripper.setPosition(l_position);
 }
  @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);
  }
Exemple #30
0
  public void moveBackward(double pow, int encoderVal, int timeout) throws InterruptedException {
    //        sensor.resetGyro();
    double angle;
    double startAngle = Math.abs(sensor.getGyroYaw());
    opMode.telemetry.update();

    double error;
    double power;

    motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();

    motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();

    setNullValue();

    nullValue = 0;

    ElapsedTime time = new ElapsedTime();
    time.startTime();

    int currentEncoder = getEncoderAvg() - nullValue;
    while (encoderVal > currentEncoder && time.milliseconds() < timeout) {
      opMode.telemetry.update();
      angle = Math.abs(sensor.getGyroYaw());

      currentEncoder = getEncoderAvg() - nullValue;

      error = (double) (encoderVal - currentEncoder) / encoderVal;

      power = (pow * error) - .25;

      Range.clip(power, -1, 1);

      opMode.telemetry.addData("Power", power);
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

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

      startMotors(power, power);
      opMode.idle();
    }
    stopMotors();
    opMode.telemetry.update();
    angleError = sensor.getGyroYaw();
  }