Пример #1
0
 /**
  * Calculate the velocity after executing the acceleration schedule, starting from the initial
  * velocity. The last acceleration will be ignored.
  *
  * @param v1 the initial velocity
  * @return the velocity after executing the acceleration schedule
  */
 public double calcFinalVelocity(double v1) {
   TimeAccel lastTa = null;
   for (TimeAccel ta : timeAccelList) {
     if (lastTa != null) {
       v1 += (ta.getTime() - lastTa.getTime()) * lastTa.getAcceleration();
     }
     lastTa = ta;
   }
   return v1;
 }
Пример #2
0
  /**
   * Calculate the distance and velocity traveled by the vehicle starting at a given time and
   * velocity and ending at a given time.
   *
   * @param time1 the initial time
   * @param v1 the initial velocity
   * @param timeEnd the final time
   * @return the velocity after executing the acceleration schedule after a period of time t
   */
  public double[] calcFinalDistanceAndVelocity(double time1, double v1, double timeEnd) {
    assert time1 < timeEnd;

    int i = 0;
    double accel = 0.0;

    while (i < timeAccelList.size()) {
      TimeAccel ta = timeAccelList.get(i);
      if (time1 < ta.getTime()) break;
      accel = ta.getAcceleration();
      i++;
    }

    double time = time1;
    double v = v1;
    double dTotal = 0.0;

    while (i < timeAccelList.size()) {
      TimeAccel ta = timeAccelList.get(i);
      if (timeEnd < ta.getTime()) break; // can be <=
      double t = ta.getTime() - time;
      double v2 = v + accel * t;
      dTotal += t * (v + v2) / 2.0;
      time = ta.getTime();
      v = v2;
      accel = ta.getAcceleration();
      i++;
    }

    double t = timeEnd - time;
    double v2 = v + accel * t;
    dTotal += t * (v + v2) / 2.0;

    return new double[] {dTotal, v2};
  }
Пример #3
0
 /** {@inheritDoc} */
 @Override
 public String toString() {
   String s = "";
   boolean isFirst = true;
   for (TimeAccel ta : timeAccelList) {
     if (isFirst) {
       s += "[";
       isFirst = false;
     } else {
       s += ",";
     }
     s += ta.toString();
   }
   s += "]";
   return s;
 }
Пример #4
0
 /**
  * Whether the velocity does not exceed the velocity's upper limit at any time point during the
  * execution of the acceleration schedule
  *
  * @param v1 the initial velocity
  * @param vTop the velocity's upper limit
  * @return Whether the velocity does not exceed the velocity's upper limit at all time during the
  *     execution of this acceleration schedule.
  */
 public boolean checkVelocityUpperLimit(double v1, double vTop) {
   if (!Util.isDoubleEqualOrLess(v1, vTop)) {
     return false;
   }
   TimeAccel lastTa = null;
   for (TimeAccel ta : timeAccelList) {
     if (lastTa != null) {
       v1 += (ta.getTime() - lastTa.getTime()) * lastTa.getAcceleration();
       if (Util.isDoubleNotEqual(v1, vTop) && v1 > vTop) {
         return false;
       }
     }
     lastTa = ta;
   }
   return true;
 }
Пример #5
0
  /**
   * Calculate the distance and velocity traveled by the vehicle starting at a given time and
   * velocity and ending at a given time.
   *
   * @param time1 the initial time
   * @param v1 the initial velocity
   * @param dTotal the distance traveled
   * @return the velocity after executing the acceleration schedule after a period of time t
   */
  public double[] calcFinalTimeAndVelocity(double time1, double v1, double dTotal) {
    int i = 0;
    double accel = 0.0;

    while (i < timeAccelList.size()) {
      TimeAccel ta = timeAccelList.get(i);
      if (time1 < ta.getTime()) break;
      accel = ta.getAcceleration();
      i++;
    }

    double time = time1;
    double v = v1;
    double dSum = 0.0;

    while (i < timeAccelList.size()) {
      TimeAccel ta = timeAccelList.get(i);
      double t = ta.getTime() - time;
      double v2 = v + accel * t;
      double d = t * (v + v2) / 2.0;
      if (dSum + d >= dTotal) break;
      dSum += d;
      time = ta.getTime();
      v = v2;
      accel = ta.getAcceleration();
      i++;
    }

    // Solving:
    //   vEnd = v + accel * t
    //   d = t * (v + vEnd) / 2
    // Solutions:
    //   d = ((vEnd - v) / accel) * (v + vEnd) / 2
    //   d = (vEnd^2 - v^2) / (2 * accel)
    //   2 * accel * d = vEnd^2 - v^2
    //   vEnd^2 = 2 * accel * d + v^2
    //   vEnd = sqrt(2 * accel *d + v^2)
    //   t = (vEnd - v) / accel

    double d = dTotal - dSum;
    if (d > 0.0) {
      if (Util.isDoubleZero(accel)) {
        return null; // cannot accelerate to finish the remaining distance
      } else {
        if (2.0 * accel * d + v * v >= 0.0) {
          double vEnd = Math.sqrt(2.0 * accel * d + v * v);
          double t = (vEnd - v) / accel;
          double timeEnd = time + t;

          return new double[] {timeEnd, vEnd};
        } else {
          return null; // decelerate too quickly and cannot finish the
          // remaining distance
        }
      }
    } else {
      assert Util.isDoubleZero(d);
      return new double[] {time, v}; // no need to move further
    }
  }