public void smoothDump(ElapsedTime timer) {
   moveStraight(8.5, false, .3);
   double pos = Keys.CLIMBER_INITIAL_STATE;
   // .85 to .31 so you want to decrement
   timer.reset();
   telemetry.addData("place", "before while");
   while (pos > Keys.CLIMBER_DUMP) {
     timer.reset();
     while (timer.time() * 1000 < 200) {
       // do nothing
       try {
         sleep(2);
       } catch (InterruptedException e) {
         e.printStackTrace();
       }
       telemetry.addData("waiting", timer.time() * 1000);
     }
     pos -= .05;
     climber.setPosition(pos);
     /*telemetry.addData("place","during while");
     telemetry.addData("timer",timer.time());
     telemetry.addData("timer math",((int)(timer.time()*1000)));
     telemetry.addData("timer math2",((int)(timer.time()*1000))%200);
     telemetry.addData("climber",climber.getPosition());
     telemetry.addData("constant","INIT"+Keys.CLIMBER_INITIAL_STATE+" DUMP"+Keys.CLIMBER_DUMP);*/
   }
   // once it is here, it finished dumping.
   // retract - the sudden should be ok cuz hopefully by that time it will have already dumped
   climber.setPosition(Keys.CLIMBER_INITIAL_STATE);
   moveStraight(8.5, true, .3);
   telemetry.addData("place", "after while");
 }
  @Override
  public void loop() {

    ElapsedTime timer = new ElapsedTime();
    double timeLimit = 3;

    /*		if (gamepad1.dpad_up) {
    	robot.barGrabberServo.goHome();
    }

    if (gamepad1.dpad_down) {
    	robot.barGrabberServo.goGrabBar();
    }

          if (gamepad1.dpad_up) {
              // go at servo direct
          }

          if (gamepad1.dpad_down) {
              // go at servo direct
          }*/
    for (int i = 0; i < 11; i++) {
      if (timer.time() > timeLimit) {
        timer.reset();
        robot.leftZipLineServo.setPosition(i / 10);
      }
    }

    telemetry.addData(
        "servo", "positiom " + String.format("%.2f", robot.leftZipLineServo.getPosition()));
  }
 /*
  * handleBattery
  *
  * The Matrix controller has a separate battery whose voltage can be read.
  */
 protected void handleBattery() {
   if ((firstBattery) || (spamPrevention.time() > SPAM_PREVENTION_FREQ)) {
     battery = mc.getBattery();
     spamPrevention.reset();
     firstBattery = false;
   }
   telemetry.addData("Battery: ", ((float) battery / 1000));
 }
  @Override
  public boolean timeslice() {
    if (timer.time() >= timeout) {
      new SingleShotTimerEvent(this, EventKind.EXPIRED).postEvent();

      return true; // Task is done, will be deleted.
    } else return false; // Task needs to keep running.
  }
 /*
  * handleServos
  *
  * Oscillate the servos.
  */
 protected void handleServos() {
   if ((firstServos) || (servoOscTimer.time() > SERVO_OSC_FREQ)) {
     if (servoPosition == 0.0) {
       servoPosition = 1.0;
     } else {
       servoPosition = 0.0;
     }
     servo1.setPosition(servoPosition);
     servo2.setPosition(servoPosition);
     servo3.setPosition(servoPosition);
     servo4.setPosition(servoPosition);
     servoOscTimer.reset();
     firstServos = false;
   }
 }
  /*
   * handleMotors
   *
   * Oscillate the motors.
   */
  protected void handleMotors() {
    if ((firstMotors) || (motorOscTimer.time() > MOTOR_OSC_FREQ)) {
      motorPower = -motorPower;

      /*
       * The MatrixDcMotorController's setMotorPower() method may take
       * a collection of motors.  If this is chosen, then the controller will
       * set a pending bit.  The pending bit tells the controller to
       * defer turning on, or changing the current set point, for a motor
       * until the pending bit is cleared.
       *
       * When the pending bit is cleared all motor power values are applied
       * simultaneously.  setMotorPower() handles the pending bit for you.
       */
      mc.setMotorPower(motorSet, motorPower);
      motorOscTimer.reset();
      firstMotors = false;
    }
  }
  /**
   * Update the angles using the raw readings.
   *
   * @param rawX raw rotation along x axis
   * @param rawY raw rotation along y axis
   * @param rawZ raw rotation along z axis
   */
  public void update(int rawX, int rawY, int rawZ) {
    if (stopWatch == null) {
      // first time?  initialize the stop watch
      stopWatch = new ElapsedTime();
    } else {
      // integrate values
      double elapsed = stopWatch.time();

      pitch += ((rawX + prevX) / 2.0) * elapsed;
      roll += ((rawY + prevY) / 2.0) * elapsed;
      heading += ((rawZ + prevZ) / 2.0) * elapsed;

      stopWatch.reset();
    }

    // remember these readings
    prevX = rawX;
    prevY = rawY;
    prevZ = rawZ;
  }
  @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();
  }
 public void getTime() {
   telemetry.addData("time", elapsedTime.time());
 }
  @Override
  public void runOpMode() throws InterruptedException {

    lwa = hardwareMap.dcMotor.get("leftwheelA");
    lwb = hardwareMap.dcMotor.get("leftwheelB");
    rwa = hardwareMap.dcMotor.get("rightwheelA");
    rwb = hardwareMap.dcMotor.get("rightwheelB");
    rwa.setDirection(DcMotor.Direction.REVERSE);
    rwb.setDirection(DcMotor.Direction.REVERSE);
    liftL = hardwareMap.dcMotor.get("liftL");
    liftR = hardwareMap.dcMotor.get("liftR");
    // liftR.setDirection(DcMotor.Direction.REVERSE);
    // liftL.setDirection(DcMotor.Direction.REVERSE);
    collector = hardwareMap.dcMotor.get("collector");
    // rightCR = hardwareMap.servo.get("rightCR");
    // leftCR = hardwareMap.servo.get("leftCR");
    swivel = hardwareMap.servo.get("swivel");
    dump = hardwareMap.servo.get("dump");
    trigL = hardwareMap.servo.get("trigL");
    trigR = hardwareMap.servo.get("trigR");
    dds = hardwareMap.servo.get("dds");
    holdL = hardwareMap.servo.get("holdL");
    holdR = hardwareMap.servo.get("holdR");
    holdC = hardwareMap.servo.get("holdC");
    // leftPivot = hardwareMap.servo.get("leftPivot");
    // rightPivot = hardwareMap.servo.get("rightPivot");
    lineSensor = hardwareMap.opticalDistanceSensor.get("dist1");
    touch = hardwareMap.touchSensor.get("touch");
    Gyro = hardwareMap.gyroSensor.get("Gyro");

    //  leftPivot.setPosition(1);
    // rightPivot.setPosition(0);
    dump.setPosition(0);
    trigL.setPosition(0.8);
    trigR.setPosition(0.05);
    //   leftCR.setPosition(0.5);
    //     rightCR.setPosition(0.5);
    dds.setPosition(1);
    holdL.setPosition(0.75);
    holdR.setPosition(0.05);
    holdC.setPosition(1);
    double lineSensorValue;

    double heading = 0;
    //    double integratedHeading = Gyro.getIntegratedZValue();
    double headingError;
    double targetHeading;
    double drivesteering;
    double driveGain = 0.005;
    double leftPower;
    double rightPower;
    double midPower = 0;
    double minPower = 0.2;

    Gyro.calibrate();

    // wait for the start button to be pressed
    waitForStart();

    timer = new ElapsedTime();

    collector.setPower(0);

    while (rwa.getCurrentPosition() > -7700 && timer.time() < 15) {
      rwa.setPower(-0.75);
      rwb.setPower(-0.75);
      lwa.setPower(-0.75);
      lwb.setPower(-0.75);
    }

    rwa.setPower(0);
    rwb.setPower(0);
    lwa.setPower(0);
    lwb.setPower(0);

    sleep(100);

    heading = Gyro.getHeading();
    telemetry.addData("heading", heading);

    heading = 359;
    sleep(100);

    while (heading > 225) {
      heading = Gyro.getHeading();
      //          integratedHeading = Gyro.getIntegratedZValue();

      telemetry.addData("heading", heading);
      //        telemetry.addData("Integrated", integratedHeading);

      targetHeading = 225;

      headingError = targetHeading - heading;

      drivesteering = driveGain * headingError;

      if (drivesteering > 1) {
        drivesteering = 1;
        telemetry.addData("Caught illegal value", "reset drivesteering to 1");
      }

      leftPower = midPower + drivesteering;
      rightPower = midPower + drivesteering;

      if (leftPower > minPower) {
        leftPower = minPower;
      }
      if (rightPower < minPower) {
        rightPower = minPower;
      }

      lwa.setPower(leftPower);
      lwb.setPower(leftPower);
      rwa.setPower(rightPower);
      rwb.setPower(rightPower);
    }

    while (heading < 225) {
      heading = Gyro.getHeading();
      //          integratedHeading = Gyro.getIntegratedZValue();

      telemetry.addData("heading", heading);
      //        telemetry.addData("Integrated", integratedHeading);

      targetHeading = 225;

      headingError = targetHeading - heading;

      drivesteering = driveGain * headingError;

      if (drivesteering > 1) {
        drivesteering = 1;
        telemetry.addData("Caught illegal value", "reset drivesteering to 1");
      }

      rightPower = midPower - drivesteering;
      leftPower = midPower + drivesteering;

      if (rightPower > minPower) {
        rightPower = minPower;
      }
      if (leftPower < minPower) {
        leftPower = minPower;
      }

      lwa.setPower(leftPower);
      lwb.setPower(leftPower);
      rwa.setPower(rightPower);
      rwb.setPower(rightPower);
    }

    lwa.setPower(1);
    lwb.setPower(1);
    rwa.setPower(1);
    rwb.setPower(1);
    sleep(400);

    lwa.setPower(0);
    lwb.setPower(0);
    rwa.setPower(0);
    rwb.setPower(0);
    telemetry.addData("Event", "Done");
    sleep(100);

    while (!touch.isPressed() && timer.time() < 20) {

      lineSensorValue = lineSensor.getLightDetectedRaw();

      if (lineSensorValue < 10) {

        lwa.setPower(0.5);
        lwb.setPower(0.5);
        rwa.setPower(0.0);
        rwb.setPower(0.0);
      } else {
        lwa.setPower(0.0);
        lwb.setPower(0.0);
        rwa.setPower(0.5);
        rwb.setPower(0.5);
      }
    }

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);
    sleep(100);

    collector.setPower(0.0);
    sleep(100);

    if (timer.time() < 20) {
      lwa.setPower(-0.35);
      lwb.setPower(-0.35);
      rwa.setPower(-0.35);
      rwb.setPower(-0.35);

      sleep(225);

      lwa.setPower(0.0);
      lwb.setPower(0.0);
      rwa.setPower(0.0);
      rwb.setPower(0.0);

      sleep(100);
      dds.setPosition(0);

      sleep(900);
    }

    lwa.setPower(-1);
    lwb.setPower(-1);
    rwa.setPower(-1);
    rwb.setPower(-1);

    sleep(500);

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);

    while (heading > 130) {
      heading = Gyro.getHeading();
      telemetry.addData("heading", heading);

      rwa.setPower(0.5);
      rwb.setPower(0.5);
      lwa.setPower(-0.5);
      lwb.setPower(-0.5);
    }

    lwa.setPower(-0.5);
    lwb.setPower(-0.5);
    rwa.setPower(-0.5);
    rwb.setPower(-0.5);
    sleep(1000);
    dds.setPosition(1);

    lwa.setPower(0.0);
    lwb.setPower(0.0);
    rwa.setPower(0.0);
    rwb.setPower(0.0);

    sleep(500);
  }
    public void run() {
      RobotLog.v("EventLoopRunnable has started");

      try {
        ElapsedTime elapsedEventLoop = new ElapsedTime();
        double sMinLoopInterval = 0.001D;
        long msLoopIntervalStep = 5L;

        while (!Thread.interrupted()) {
          while (elapsedEventLoop.time() < sMinLoopInterval) {
            Thread.sleep(msLoopIntervalStep);
          }

          elapsedEventLoop.reset();
          if (RobotLog.hasGlobalErrorMsg()) {
            EventLoopManager.this.buildAndSendTelemetry(
                "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
          }

          if (EventLoopManager.this.elapsedSinceHeartbeatReceived.startTime() == 0.0D) {
            Thread.sleep(500L);
          } else if (EventLoopManager.this.elapsedSinceHeartbeatReceived.time() > 2.0D) {
            EventLoopManager.this.handleDroppedConnection();
            EventLoopManager.this.currentPeerAddressAndPort = null;
            EventLoopManager.this.elapsedSinceHeartbeatReceived = new ElapsedTime(0L);
          }

          Iterator syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();
          SyncdDevice syncdDevice;
          while (syncdDeviceIterator.hasNext()) {
            syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
            syncdDevice.blockUntilReady();
          }

          boolean unblockOnException = false;

          try {
            unblockOnException = true;
            EventLoopManager.this.eventLoop.loop();
            unblockOnException = false;
          } catch (Exception e) {
            RobotLog.e("Event loop threw an exception");
            RobotLog.logStacktrace(e);
            String exceptionMessage =
                e.getClass().getSimpleName()
                    + (e.getMessage() != null ? " - " + e.getMessage() : "");
            RobotLog.setGlobalErrorMsg(
                "User code threw an uncaught exception: " + exceptionMessage);
            EventLoopManager.this.buildAndSendTelemetry(
                "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
            throw new RobotCoreException("EventLoop Exception in loop()");
          } finally {
            if (unblockOnException) {
              Iterator var9 = EventLoopManager.this.syncdDevices.iterator();

              while (var9.hasNext()) {
                SyncdDevice var10 = (SyncdDevice) var9.next();
                var10.startBlockingWork();
              }
            }
          }

          syncdDeviceIterator = EventLoopManager.this.syncdDevices.iterator();

          while (syncdDeviceIterator.hasNext()) {
            syncdDevice = (SyncdDevice) syncdDeviceIterator.next();
            syncdDevice.startBlockingWork();
          }
        }
      } catch (InterruptedException var20) {
        RobotLog.v("EventLoopRunnable interrupted");
        EventLoopManager.this.reportRobotStatus(RobotState.STOPPED);
      } catch (RobotCoreException var21) {
        RobotLog.v("RobotCoreException in EventLoopManager: " + var21.getMessage());
        EventLoopManager.this.reportRobotStatus(RobotState.EMERGENCY_STOP);
        EventLoopManager.this.buildAndSendTelemetry(
            "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
      }

      try {
        EventLoopManager.this.eventLoop.teardown();
      } catch (Exception var17) {
        RobotLog.w("Caught exception during looper teardown: " + var17.toString());
        RobotLog.logStacktrace(var17);
        if (RobotLog.hasGlobalErrorMsg()) {
          EventLoopManager.this.buildAndSendTelemetry(
              "SYSTEM_TELEMETRY", RobotLog.getGlobalErrorMsg());
        }
      }

      RobotLog.v("EventLoopRunnable has exited");
    }