/**
   * Rotate the robot on spot (differential heading) with a desired heading.
   *
   * @param angle angle for rotation
   * @return false in case the rotation was interrupted, true otherwise
   */
  public boolean setDiffHeading(double angle) {
    if (angle == 0) return true;

    stop = false;
    boolean ret = true;
    /* get the current heading */
    double currentHead = transformAngle(device.getYaw());
    /* calculate the goal heading */
    double newGoal = transformAngle(currentHead + angle);

    setGoal(newGoal);

    double now = transformAngle(device.getYaw());

    /* keep rotating while the goal was not reached */
    while (now != newGoal) {
      if (stop == true) {
        ret = false;
        break;
      }

      /* no point in rotating at all if we're at +/-180 */
      if (Math.abs(now - newGoal) <= 1 && newGoal == 180)
        break; /* exit if we reached our destination */

      /* in case a diff. of maxError (default 0) between angles is acceptable */
      if (Math.abs(now - newGoal) <= maxError) break; /* exit if we reached our destination */

      /* get the current heading */
      now = transformAngle(device.getYaw());

      /* get the motor command and check if within the desired limits */
      double command = getCommand(now);
      command = boundCommand(command);
      device.setSpeed(0, command);

      try {
        Thread.sleep(100);
      } catch (Exception e) {
      }
      if (isDebugging)
        System.err.println("[HeadingControl][Debug] Angle left : " + Math.abs(now - newGoal));
    }
    device.setSpeed(0, 0); /* stop the robot from rotating */

    return ret;
  }
  /**
   * Rotate the robot on spot (absolute heading) to the desired heading.
   *
   * @param angle goal angle
   * @return false in case the rotation was interrupted, true otherwise
   */
  public boolean setHeading(double angle) {
    /* get the current heading */
    double currentAngle = transformAngle(device.getYaw());

    /* difference between the current heading and the goal heading */
    double deltaAngle = (angle - currentAngle);

    if (deltaAngle != 0) {
      if (deltaAngle <= 180 && deltaAngle > 0) return setDiffHeading(deltaAngle);
      else if (deltaAngle > -180) return setDiffHeading(-360 + deltaAngle);
      else return setDiffHeading(360 + deltaAngle);
    }
    return true;
  }