public static void main(String[] args) {

    for (double absMeasure = 0.0; absMeasure < 100.0; absMeasure += 1.0) {
      double angleD = 10.0;

      System.out.print(Utils.dTS(angleD));

      // double absMeasure = Math.abs(situation.getMeasure());

      if (absMeasure >= 20.0 && absMeasure < 35.0) {
        angleD *= 1.0 + ((absMeasure - 20.0) / (35.0 - 20.0));

      } else if (absMeasure >= 35.0 && absMeasure < 60.0) {
        angleD *= 2.0 + ((absMeasure - 35.0) / (60.0 - 35.0));

      } else if (absMeasure >= 60.0) {
        angleD *= 3.0;
      }
      System.out.println(" " + Utils.dTS(absMeasure) + " " + angleD);
    }
  }
  @Override
  public void execute(SensorData data, ModifiableAction action) {
    // steering based on the "biggest sensor value" heuristic
    double[] rawSensors = data.getRawTrackEdgeSensors();
    int index = 0;
    double longest = rawSensors[0];
    for (int i = 1; i < rawSensors.length; ++i) {
      if (rawSensors[i] > longest) {
        index = i;
        longest = rawSensors[i];
      }
    }

    double angleD = -angles[index];

    if (TEXT_DEBUG) {
      System.out.println(
          Utils.timeToExactString(data.getCurrentLapTime())
              + " @ "
              + data.getDistanceRacedS()
              + "- track: "
              + data.getTrackPositionS()
              + ", index: "
              + index
              + ", angleD: "
              + Utils.dTS(angleD));
    }

    double absMeasure = Math.abs(situation.getMeasure());

    if (absMeasure >= 20.0 && absMeasure < 35.0) {
      angleD *= 1.0 + ((absMeasure - 20.0) / (35.0 - 20.0));

    } else if (absMeasure >= 35.0 && absMeasure < 60.0) {
      angleD *= 2.0 + ((absMeasure - 35.0) / (60.0 - 35.0));

    } else if (absMeasure >= 60.0) {
      angleD *= 3.0;
    }

    if (TEXT_DEBUG) {
      System.out.println("absMeasure: " + Utils.dTS(absMeasure) + ", angleD: " + Utils.dTS(angleD));
    }

    double trackPos = data.getTrackPosition();
    double trackAngle = data.getAngleToTrackAxis();

    if (situation.isCorner()) {
      boolean mirror = situation.isRight();

      if (mirror) {
        trackPos *= -1.0;
        trackAngle *= -1.0;
        angleD *= -1.0;
        index = 18 - index;
      }

      double absTrackPos = SensorData.calcAbsoluteTrackPosition(trackPos, trackWidth);

      // inside
      if (absTrackPos - (CAR_WIDTH * 0.5) < INSIDE_MARGIN && trackAngle < 0.0) {
        double beta = (INSIDE_MARGIN - (absTrackPos - (CAR_WIDTH * 0.5))) / INSIDE_MARGIN;
        if (TEXT_DEBUG) {
          System.out.println("Inside");
        }

        // not so close -> open steering
        if (beta < 0.5) {
          // System.out.println("IN - Opening@"+Utils.dTS(data.getDistanceFromStartLine()));
          if (TEXT_DEBUG) {
            System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
          }
          double alpha = (beta - 0.5) * -(1 / 0.5);
          angleD *= alpha;
          if (TEXT_DEBUG) {
            System.out.println(", alpha: " + Utils.dTS(alpha) + ", new: " + Utils.dTS(angleD));
          }

        } else {
          // System.out.println("IN - Other@"+Utils.dTS(data.getDistanceFromStartLine()));
          beta = Math.min(beta, 1.0);
          // very close, steer in the other direction
          if (TEXT_DEBUG) {
            System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
          }
          double alpha = (beta - 0.5) * (1 / 0.5);
          angleD = Math.toDegrees(trackAngle) * alpha;
          // angleD = 2.0*alpha;
          if (TEXT_DEBUG) {
            System.out.println(", alpha: " + Utils.dTS(alpha) + ", new: " + Utils.dTS(angleD));
          }
        }
      }

      // outside
      if (absTrackPos + (CAR_WIDTH * 0.5) > trackWidth - OUTSIDE_MARGIN) {
        // System.out.println("OUT@"+Utils.dTS(data.getDistanceFromStartLine()));
        if (TEXT_DEBUG) {
          System.out.println("Outside");
        }
        // increase steering angle
        double alpha = Math.min(absMeasure, OUTSIDE_DOUBLE) / OUTSIDE_DOUBLE;
        double beta =
            ((absTrackPos + (CAR_WIDTH * 0.5)) - (trackWidth - OUTSIDE_MARGIN)) / OUTSIDE_MARGIN;

        if (TEXT_DEBUG) {
          System.out.print("Beta: " + Utils.dTS(beta) + ", old: " + Utils.dTS(angleD));
        }

        beta = Math.min(beta, 1.0);
        angleD *= 1.0 + (alpha * beta);

        if (TEXT_DEBUG) {
          System.out.println(
              ", 1+alpha: " + Utils.dTS(1.0 + alpha) + ", new: " + Utils.dTS(angleD));
        }
      }

      // if(Math.abs(Math.toDegrees(trackAngle)) < 10.0){
      //    System.out.println(absMeasure + ";" + trackPos + ";" + Math.toDegrees(trackAngle) + ";"
      // + index + ";" + angleD);
      // }

      if (mirror) {
        angleD *= -1.0;
      }
    }

    angleD = Math.min(45.0, Math.max(-45.0, angleD));
    action.setSteering(angleD / 45.0);
    action.limitValues();
    if (TEXT_DEBUG) {
      System.out.println("Final value: " + action.getSteeringS());
    }
  }
Ejemplo n.º 3
0
  private PlanElement2011 plan(
      PlanStackData planData,
      SensorData data,
      TrackModel trackModel,
      OpponentObserver observer,
      boolean planning) {
    int index = planData.currentSegment();
    TrackSegment current = trackModel.getSegment(index);
    TrackSegment next = trackModel.getSegment(trackModel.incrementIndex(index));
    TrackSegment prev = trackModel.getSegment(trackModel.decrementIndex(index));

    double end = planData.end();
    double start = planData.start();

    if (Plan.TEXT_DEBUG && planning) {
      plan.println("");
      plan.println("------------ PlanStraight------------");
      plan.println(
          "Start/End: "
              + Utils.dTS(start)
              + ", "
              + Utils.dTS(end)
              + " ["
              + Utils.dTS(end - start)
              + "m]"
              + " - ");
      planData.print();
      plan.println("");
    }

    // plan like the last element?
    boolean planLikeLast = current.contains(data.getDistanceFromStartLine());

    if (planLikeLast) {
      start = data.getDistanceRaced();

      if (Plan.TEXT_DEBUG && planning) {
        plan.println("This is the last segment to plan for...");
      }
    }

    // check if we can combine the planning of this segment and the previous
    if (!planLikeLast && prev.isStraight()) {
      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Previous is also straight, checking further...");
      }
      if (start - prev.getLength() <= data.getDistanceRaced()) {
        if (Plan.TEXT_DEBUG && planning) {
          plan.println("Prev is the last segment to plan for");
        }
        start = data.getDistanceRaced();
        planLikeLast = true;

      } else {
        if (Plan.TEXT_DEBUG && planning) {
          plan.println("Prev is also a middle segment");
        }
        start -= prev.getLength();
        int prevIndex = trackModel.decrementIndex(index);
        int prevPrevIndex = trackModel.decrementIndex(prevIndex);
        if (Plan.TEXT_DEBUG && planning) {
          plan.println("Switching prev from " + prevIndex + " to " + prevPrevIndex);
        }
        prev = trackModel.getSegment(prevPrevIndex);
      }

      if (Plan.TEXT_DEBUG && planning) {
        plan.println(
            "Moving start to " + Utils.dTS(start) + ", new length " + Utils.dTS(end - start) + "m");
      }
      planData.popSegment();
    }

    double brakeDistance = 0.0;

    if (planData.approachSpeed != Plan2013.MAX_SPEED) {
      if (planData.first()) {
        brakeDistance =
            plan.calcBrakingZoneStraight(data.getSpeed(), end - start, planData.approachSpeed);
      } else {
        brakeDistance =
            plan.calcBrakingZoneStraight(planData.speed(), end - start, planData.approachSpeed);
      }
    }

    if (Plan.TEXT_DEBUG && planning) {
      plan.println("Brake distance: " + Utils.dTS(brakeDistance) + "m");
    }

    Point2D targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;

    if (observer.otherCars()
        && observer.getRecommendedPosition() != OpponentObserver.NO_RECOMMENDED_POINT) {
      Point2D recommendation = new Point2D.Double();
      recommendation.setLocation(observer.getRecommendedPosition());

      if (start <= recommendation.getY() && recommendation.getY() < end) {
        targetPosition = new Point2D.Double();
        targetPosition.setLocation(recommendation);
      }
    }

    // track positions
    ArrayList<Interpolator> positions = new ArrayList<>();

    double currStart = start;
    double remainingLength = end - start;
    double currPosition = plan.getAnchorPoint(prev, current);
    if (planLikeLast) {
      currPosition = data.getTrackPosition();
    }

    if (Plan.TEXT_DEBUG && planning) {
      plan.println(Utils.dTS(remainingLength) + "m remain...");
    }

    if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
      if (Plan.TEXT_DEBUG && planning) {
        plan.println("I need to take care of other cars, point is " + targetPosition.toString());
        plan.println("Right now, i'm planning with currStart: " + Utils.dTS(currStart));
      }
      if (targetPosition.getY() < currStart) {
        if (Plan.TEXT_DEBUG && planning) {
          plan.println("Point is before currStart, moving it to +1m");
        }
        targetPosition.setLocation(targetPosition.getX(), targetPosition.getY() + 1.0);
      }

      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Checking, if there is enough room...");
      }

      double lengthNeeded = targetPosition.getY() - currStart;

      if (Plan.TEXT_DEBUG && planning) {
        plan.println(
            "I need at least "
                + Utils.dTS(lengthNeeded)
                + "m, "
                + Utils.dTS(remainingLength)
                + "m remain");
      }

      if (remainingLength < lengthNeeded) {
        targetPosition = OpponentObserver.NO_RECOMMENDED_POINT;
        if (Plan.TEXT_DEBUG && planning) {
          plan.println("Cannot overtake, not enough room");
        }
      }
    }

    if (targetPosition != OpponentObserver.NO_RECOMMENDED_POINT) {
      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Trying to overtake");
      }

      // switch Position 1
      double[] xP = new double[3];
      double[] yP = new double[3];

      xP[0] = currStart;
      xP[2] = targetPosition.getY();
      xP[1] = (xP[0] + xP[2]) / 2.0;

      yP[0] = currPosition;
      yP[2] = targetPosition.getX();
      yP[1] = (yP[0] + yP[2]) / 2.0;

      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Switch Position 1:");
        for (int k = 0; k < xP.length; ++k) {
          plan.println(xP[k] + " , " + yP[k]);
        }
      }

      CubicSpline spline = new CubicSpline(xP, yP);
      spline.setDerivLimits(0.0, 0.0);
      positions.add(new FlanaganCubicWrapper(spline));

      currStart = xP[2];
      remainingLength = end - currStart;
      currPosition = targetPosition.getX();

      // overtaking
      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Overtaking line");
      }

      xP = new double[3];

      xP[0] = currStart;
      xP[2] = end - 150.0;

      if (Plan.TEXT_DEBUG && planning) {
        plan.println(
            "Overtaking line: "
                + Utils.dTS(currStart)
                + " "
                + Utils.dTS(end - 150.0)
                + " "
                + targetPosition.getX());
      }

      positions.add(new ConstantValue(xP[0], xP[2], targetPosition.getX()));

      currStart = xP[2];
      remainingLength = end - currStart;
      currPosition = targetPosition.getX();
    }

    // simply drive towards the target position for the next corner
    if (Plan.TEXT_DEBUG && planning) {
      plan.println("Planning towards the next corner...");
    }

    double[] xP = new double[3];
    double[] yP = new double[3];

    xP[0] = currStart;
    xP[2] = end;
    xP[1] = (xP[0] + xP[2]) / 2.0;

    yP[0] = currPosition;

    double absCurrPosition =
        SensorData.calcAbsoluteTrackPosition(currPosition, trackModel.getWidth());
    double possibleDelta = Plan2013.calcPossibleSwitchDelta(data, end - currStart);
    double anchor = plan.getAnchorPoint(current, next);
    double absDesiredPosition = SensorData.calcAbsoluteTrackPosition(anchor, trackModel.getWidth());

    if (Plan.TEXT_DEBUG && planning) {
      plan.println("absCurrPosition: " + Utils.dTS(absCurrPosition));
      plan.println("PossibleDelta: " + Utils.dTS(possibleDelta));
      plan.println("absDesiredPosition: " + Utils.dTS(absDesiredPosition));
    }

    if (Math.abs(absDesiredPosition - absCurrPosition) <= possibleDelta) {
      if (Plan.TEXT_DEBUG && planning) {
        plan.println("Positioning ok");
      }
      yP[2] = plan.getAnchorPoint(current, next);

    } else {
      // move to the right
      if (anchor < 0) {
        yP[2] =
            SensorData.calcRelativeTrackPosition(
                absCurrPosition + possibleDelta, trackModel.getWidth());

      } else {
        // move to the left
        yP[2] =
            SensorData.calcRelativeTrackPosition(
                absCurrPosition - possibleDelta, trackModel.getWidth());
      }
    }

    yP[1] = (yP[0] + yP[2]) / 2.0;

    if (Plan.TEXT_DEBUG && planning) {
      plan.println("Position towards the next corner:");
      for (int k = 0; k < xP.length; ++k) {
        plan.println(xP[k] + " , " + yP[k]);
      }
    }

    CubicSpline spline = new CubicSpline(xP, yP);
    spline.setDerivLimits(0.0, 0.0);
    positions.add(new FlanaganCubicWrapper(spline));

    // target speed
    double[] xS;
    double[] yS;

    if (brakeDistance >= end - start || brakeDistance == 0.0) {
      xS = new double[3];
      yS = new double[3];

      xS[0] = start;
      xS[2] = end;
      xS[1] = (xS[0] + xS[2]) / 2.0;

      if (brakeDistance == 0.0) {
        yS[0] = Plan2013.MAX_SPEED;
        yS[1] = Plan2013.MAX_SPEED;
        yS[2] = Plan2013.MAX_SPEED;
        planData.approachSpeed = Plan2013.MAX_SPEED;

      } else {
        yS[0] = planData.approachSpeed;
        yS[1] = planData.approachSpeed;
        yS[2] = planData.approachSpeed;
        planData.approachSpeed =
            plan.calcApproachSpeedStraight(planData.approachSpeed, end - start);
      }

    } else {
      xS = new double[4];
      yS = new double[4];

      xS[0] = start;
      xS[1] = end - brakeDistance;
      xS[2] = end - (brakeDistance * 0.99);
      xS[3] = end;

      yS[0] = Plan2013.MAX_SPEED;
      yS[1] = Plan2013.MAX_SPEED;
      yS[2] = planData.approachSpeed;
      yS[3] = planData.approachSpeed;

      planData.approachSpeed = Plan2013.MAX_SPEED;
    }

    if (Plan.TEXT_DEBUG && planning) {
      plan.println("Speed:");
      for (int i = 0; i < xS.length; ++i) {
        plan.println(xS[i] + " , " + yS[i]);
      }
    }

    LinearInterpolator speed;

    try {
      speed = new LinearInterpolator(xS, yS);

    } catch (RuntimeException e) {
      System.out.println("*****************EXCEPTION**************");
      System.out.println(
          "Start/End: "
              + Utils.dTS(start)
              + ", "
              + Utils.dTS(end)
              + " ["
              + Utils.dTS(end - start)
              + "m]"
              + " - ");
      System.out.println("Segment: " + current.toString());
      System.out.println("Start: " + start);
      System.out.println("End: " + end);
      System.out.println("BrakeDistance: " + brakeDistance);
      System.out.println("Data:");
      try {
        java.io.OutputStreamWriter osw = new java.io.OutputStreamWriter(System.out);
        SensorData.writeHeader(osw);
        osw.append('\n');
        data.write(osw);
        osw.flush();

      } catch (Exception schwupp) {

      }
      System.out.println("");
      System.out.println("Speed:");
      for (int i = 0; i < xS.length; ++i) {
        System.out.println(xS[i] + " , " + yS[i]);
      }
      System.out.println("Complete Model:");
      trackModel.print();

      throw e;
    }

    PlanElement2011 element = new PlanElement2011(xS[0], xS[xS.length - 1], "Accelerate");

    for (Interpolator cs : positions) {
      element.attachPosition(cs);
    }

    element.setSpeed(speed);

    return element;
  }