public void mapBooleanVariableToComponent(
     YoVariableHolder holder,
     SaitekX52Mapping mapping,
     String variableName,
     boolean toggle,
     boolean invert) {
   BooleanYoVariable yoVariable = (BooleanYoVariable) holder.getVariable(variableName);
   if (yoVariable != null) {
     mapBooleanVariableToComponent(mapping, yoVariable, toggle, invert);
   } else {
     PrintTools.warn(this, "Variable " + variableName + " could not be found!");
   }
 }
 public void mapDoubleVariableToComponent(
     YoVariableHolder holder,
     SaitekX52Mapping mapping,
     String variableName,
     double minValue,
     double maxValue,
     double deadZone,
     boolean invert) {
   DoubleYoVariable yoVariable = (DoubleYoVariable) holder.getVariable(variableName);
   if (yoVariable != null) {
     mapDoubleVariableToComponent(mapping, yoVariable, minValue, maxValue, deadZone, invert);
   } else {
     PrintTools.warn(this, "Variable " + variableName + " could not be found!");
   }
 }
  public CameraTrackAndDollyYoVariablesHolder(YoVariableHolder holder) {
    if (holder != null) {
      if (holder.hasUniqueVariable("q_x")) {
        setTrackXVar((DoubleYoVariable) holder.getVariable("q_x"));
        setDollyXVar((DoubleYoVariable) holder.getVariable("q_x"));
      }

      if (holder.hasUniqueVariable("q_y")) {
        setTrackYVar((DoubleYoVariable) holder.getVariable("q_y"));
        setDollyYVar((DoubleYoVariable) holder.getVariable("q_y"));
      }

      if (holder.hasUniqueVariable("q_z")) {
        setTrackZVar((DoubleYoVariable) holder.getVariable("q_z"));
        setDollyZVar((DoubleYoVariable) holder.getVariable("q_z"));
      }
    }
  }
    public void run() {
      while (true) {
        final DoubleYoVariable headingDotConstant =
            (DoubleYoVariable) registry.getVariable("headingDotConstant");
        final DoubleYoVariable velocityXConstant =
            (DoubleYoVariable) registry.getVariable("velocityXConstant");
        BooleanYoVariable multisenseControlsSpeedAndHeading =
            (BooleanYoVariable) registry.getVariable("isMultisenseControllingSpeedAndHeading");

        DoubleYoVariable desiredHeadingDot =
            (DoubleYoVariable)
                registry.getVariable("RateBasedDesiredHeadingControlModule", "desiredHeadingDot");
        DoubleYoVariable desiredVelocityX =
            (DoubleYoVariable)
                registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityX");

        DoubleYoVariable multisenseHeading =
            (DoubleYoVariable) registry.getVariable("multisenseHeading");
        DoubleYoVariable multisenseMagnitude =
            (DoubleYoVariable) registry.getVariable("multisenseMagnitude");

        if (multisenseControlsSpeedAndHeading == null
            || desiredHeadingDot == null
            || desiredVelocityX == null
            || multisenseHeading == null
            || multisenseMagnitude == null) {
          continue;
        }

        if (multisenseControlsSpeedAndHeading.getBooleanValue()) {
          if (Math.abs(multisenseHeading.getDoubleValue()) > EPSILON) {
            currentHeadingDot =
                multisenseHeading.getDoubleValue() * headingDotConstant.getDoubleValue();

            if (currentHeadingDot < minHeadingDot) currentHeadingDot = minHeadingDot;
            if (currentHeadingDot > maxHeadingDot) currentHeadingDot = maxHeadingDot;
          } else {
            currentHeadingDot = 0.0;
          }

          double newVelocity =
              multisenseMagnitude.getDoubleValue() * velocityXConstant.getDoubleValue();
          // --(!) stop at half a meter from centroid of obj
          if (multisenseMagnitude.getValueAsDouble() > 0.5) {
            // -- This precaution may not be crucial, but
            // -- ramp up velocity for large jumps
            double deltaVelocity = newVelocity - currentVelocityX;
            if (Math.abs(deltaVelocity) > .1)
              currentVelocityX += deltaVelocity / Math.abs(deltaVelocity) * 0.05;
            else currentVelocityX = newVelocity;

            if (currentVelocityX < minVeclocity) currentVelocityX = minVeclocity;
            if (currentVelocityX > maxVelocity) currentVelocityX = maxVelocity;
          } else {
            currentVelocityX = 0.0;
          }

          desiredHeadingDot.set(currentHeadingDot);
          desiredVelocityX.set(currentVelocityX);
        }

        try {
          Thread.sleep(100); // 10Hz
        } catch (InterruptedException e) {
          e.printStackTrace();
        }
      }
    }