@Override
  public void init() {
    super.init();

    speedMultiplier = 1;

    powerHangingSpeed = .7f;
    pincerHangingSpeed = .3f;
    pivotHangingSpeed = .25f;

    speedReversePressed = false;
    speedChangePressed = false;
    hangPivotBrakePressed = false;
    hangBrakePressed = false;
    hangPivotPressed = false;

    // No retention on the claw yet
    keepMove = 0.0f;
    keepPivot = 0;

    hangPivot = .75f;
    servoPlease.setPosition(.5);
    LWing.setPosition(.5);
    RWing.setPosition(.5);
  }
  /** 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);
  }