private float GetAccel(ISensorModel model)
{
// checks if car is out of track
if (model.GetTrackPosition() < 1 && model.GetTrackPosition() > -1)
{
// reading of sensor at +5 degree w.r.t. car axis
float rxSensor = (float)model.GetTrackEdgeSensors()[10];
// reading of sensor parallel to car axis
float sensor = (float)model.GetTrackEdgeSensors()[9];
// reading of sensor at -5 degree w.r.t. car axis
float sxSensor = (float)model.GetTrackEdgeSensors()[8];
float targetSpeed;
// track is straight and enough far from a turn so goes to max speed
if (sensor > MaxSpeedDist || (sensor >= rxSensor && sensor >= sxSensor))
{
targetSpeed = MaxSpeed;
}
else
{
// approaching a turn on right
if (rxSensor > sxSensor)
{
float h = sensor * Sin5;
float b = rxSensor - sensor * Cos5;
float sinAngle = b * b / (h * h + b * b);
// estimate the target speed depending on turn and on how close it is
targetSpeed = MaxSpeed * (sensor * sinAngle / MaxSpeedDist);
}
// approaching a turn on left
else
{
// computing approximately the "angle" of turn
float h = sensor * Sin5;
float b = sxSensor - sensor * Cos5;
float sinAngle = b * b / (h * h + b * b);
// estimate the target speed depending on turn and on how close it is
targetSpeed = MaxSpeed * (sensor * sinAngle / MaxSpeedDist);
}
}
// accel/brake command is exponentially scaled w.r.t. the difference between target speed and current one
return (float)(2 / (1 + Math.Exp(model.GetSpeed() - targetSpeed)) - 1);
}
else
{
return 0.3f; // when out of track returns a moderate acceleration command
}
}
