public override Action Control(ISensorModel sensorModel)
{
Action action = new Action();
if(sensorModel.GetSpeed() < TargetSpeed)
{
action.Accelerate = 1;
}
if(sensorModel.GetAngleToTrackAxis() < 0)
{
action.Steering = -0.1f;
}
else
{
action.Steering = 0.1f;
}
action.Gear = 1;
return action;
}
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
}
}
private float GetSteer(ISensorModel model)
{
// steering angle is compute by correcting the actual car angle w.r.t. to track
// axis [sensors.getAngle()] and to adjust car position w.r.t to middle of track [sensors.getTrackPos()*0.5]
float targetAngle = (float)(model.GetAngleToTrackAxis() - model.GetTrackPosition() * 0.5f);
// at high speed reduce the steering command to avoid loosing the control
if (model.GetSpeed() > SteerSensitivityOffset)
{
return (float)(targetAngle / (SteerLock * (model.GetSpeed() - SteerSensitivityOffset) * WheelSensitivityCoeff));
}
else
{
return (targetAngle) / SteerLock;
}
}
private float FilterAbs(ISensorModel model, float brake)
{
// convert speed to m/s
float speed = (float)(model.GetSpeed() / 3.6f);
// when speed lower than min speed for abs do nothing
if (speed < AbsMinSpeed)
{
return brake;
}
// compute the speed of wheels in m/s
float slip = 0.0f;
for(int i = 0; i < 4; i++)
{
slip += (float)(model.GetWheelSpinVelocity()[i] * WheelRadius[i]);
}
// slip is the difference between actual speed of car and average speed of wheels
slip = speed - slip / 4.0f;
// when slip too high apply ABS
if (slip > AbsSlip)
{
brake = brake - (slip - AbsSlip) / AbsRange;
}
if(brake < 0)
{
return 0;
}
else
{
return brake;
}
}