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;
}
}
public override Action Control(ISensorModel sensorModel)
{
// check if car is currently stuck
if (Math.Abs(sensorModel.GetAngleToTrackAxis()) > StuckAngle)
{
// update stuck counter
stuck++;
}
else
{
// if not stuck reset stuck counter
stuck = 0;
}
// after car is stuck for a while apply recovering policy
if (stuck > StuckTime)
{
/* set gear and sterring command assuming car is
* pointing in a direction out of track */
// to bring car parallel to track axis
float steer = (float)(-sensorModel.GetAngleToTrackAxis() / SteerLock);
int gear = -1; // Gear R
// if car is pointing in the correct direction revert gear and steer
if (sensorModel.GetAngleToTrackAxis() * sensorModel.GetTrackPosition() > 0)
{
gear = 1;
steer = -steer;
}
clutch = Clutching(sensorModel, clutch);
// build a CarControl variable and return it
Action action = new Action();
action.Gear = gear;
action.Steering = steer;
action.Accelerate = 1.0f;
action.Brake = 0;
action.Clutch = clutch;
return action;
}
else // car is not stuck
{
// compute accel/brake command
float accelAndBrake = GetAccel(sensorModel);
// compute gear
int gear = GetGear(sensorModel);
// compute steering
float steer = GetSteer(sensorModel);
// normalize steering
if (steer < -1)
{
steer = -1;
}
if(steer > 1)
{
steer = 1;
}
// set accel and brake from the joint accel/brake command
float accel, brake;
if(accelAndBrake > 0)
{
accel = accelAndBrake;
brake = 0;
}
else
{
accel = 0;
// apply ABS to brake
brake = FilterAbs(sensorModel, -accelAndBrake);
}
clutch = Clutching(sensorModel, clutch);
// build a CarControl variable and return it
Action action = new Action();
action.Gear = gear;
action.Steering = steer;
action.Accelerate = accel;
action.Brake = brake;
action.Clutch = clutch;
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
}
}