@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());
}
}
private boolean withinCorner() {
return (segment != null && !segment.isUnknown() && segment.isCorner()) || situation.isCorner();
}