com.azalient.api.uf
Class UserDefinedFollowingModel
java.lang.Object
com.azalient.api.uf.UserDefinedFollowingModel
public class UserDefinedFollowingModel
- extends Object
An example of a user defined following model.
To use, replace these classes in commuter.jar with your own.
public class UserDefinedFollowingModel
{
private static final double IMMINENT = 3.0;
private static final double NOT_IMMINENT = 10.0;
private static final double NOT_IMMINENT_DEC = 0.5;
private static final double targetHeadwaySec = 1.2;
private static final double targetHeadwayThresholdN = 10.0;
private static final double zeroSpeedMinimumGap = 5.0;
private static final double accThresholdDistanceD1 = zeroSpeedMinimumGap + 2.0;
private static final double accThresholdDistanceD2 = zeroSpeedMinimumGap + 100.0;
private static final double kMaxDec = 0.5;
public static double freeAcceleration(IMotor vehicle, double targetSpeed, double targetDistance, double dt)
{
double speed = vehicle.speedMPS();
double gradient = vehicle.gradient();
// Accelerating
if (targetSpeed > speed)
{
double maxPhysicalAcc = vehicle.maxAcceleration(speed, gradient);
// Assume that preferred acceleration (as proportion of maximum possible)
// is maximum at zero and reduces in a parabola to 10% of max at target speed
double dv = speed / targetSpeed;
return maxPhysicalAcc * (1.0 - (0.9 * (dv * dv)));
}
else // Decelerating
{
double maxPhysicalDec = vehicle.maxDeceleration(speed, gradient);
// Assume that preferred deceleration increases as target approaches
if (targetDistance < 1.0) targetDistance = 1.0;
double secondsToImpact = targetDistance / (speed - targetSpeed);
double imminence;
if (secondsToImpact < IMMINENT) imminence = 1.0;
else if (secondsToImpact >= NOT_IMMINENT) imminence = NOT_IMMINENT_DEC;
else imminence = 1.0 - (((secondsToImpact - IMMINENT) / (NOT_IMMINENT - IMMINENT)) * (1.0 - NOT_IMMINENT_DEC));
return - imminence * maxPhysicalDec; // NEGATIVE VALUE
}
}
public static double followingAcceleration(IMotor vehicle, IMotor leader, double gap, double interval)
{
boolean sel = vehicle.selected();
// Distance between => time between
double dAB = gap;;
double aSpeed = vehicle.speedMPS();
double bSpeed = leader.speedMPS();
double acc = 0;
double preferredAcc = freeAcceleration(vehicle, bSpeed, dAB, interval);
if (aSpeed == 0)
{
// If speed is zero, then time gap is infinite, so we need to apply a different rule,
based on the distance between A and B. Define two thresholds, D1 and D2.
acc = 0 d <= D1 [if closer than D1, stay put]
acc = AccMax * ((d-D1) / (D2-D1)) D1 < d <= D2 [Linear increase]
//acc = AccMax D2 < d
//
if (dAB <= accThresholdDistanceD1) acc = 0;
else if (dAB <= accThresholdDistanceD2) acc = preferredAcc * ((dAB - accThresholdDistanceD1) / (accThresholdDistanceD2 - accThresholdDistanceD1));
else acc = preferredAcc;
}
else // non-zero speed
{
// Find position on dV/dt graph
double dt = dAB / aSpeed;
double dV = bSpeed - aSpeed;
double maxAcc = vehicle.maxAcceleration(aSpeed, vehicle.gradient());
double maxDec = vehicle.maxDeceleration(aSpeed, vehicle.gradient());
// find distance from line through (0, TH) , equation dt = TH + (1 / 2.AD).dV
// calculate the T co-ordinate on line 1 for this V
double dtv1 = targetHeadwaySec - (dV / (2.0 * maxDec));
if (dt >= dtv1) // above line 1
{
double dtMax = targetHeadwaySec * targetHeadwayThresholdN;
double dtp = 0;
double max = maxAcc;
if (dV < 0)
{
// calculate the T co-ordinate on line 1 for this V
double dtv2 = targetHeadwaySec - (dV / ( maxDec));
// calculate the T co-ordinate on line 1 for this V
double dtv3 = targetHeadwaySec - (dV / (0.5 * maxDec));
if (dt >= dtv3) // above line 3
{
dtp = dt - dtv3; // still accelerate
}
else if (dt >= dtv2)
{
max = 0; // no action
}
else // dt >= dtv1 and dt < dtv2
{
dtMax = dtv2 - dtv1;
dtp = dtv2 - dt;
max = kMaxDec * maxDec;
}
}
else
{
dtp = dt - dtv1;
}
if (dtp >= dtMax) acc = max;
else acc = (dtp/dtMax) * max;
}
else // below the line, acc should be negative
{
double dtp = dtv1 - dt;
double dtMax = targetHeadwaySec;
if (dtp > dtMax) acc = -maxDec; // Collision inevitable
else acc = (kMaxDec + ((1 - kMaxDec) * (dtp/dtMax))) * -maxDec;
// Clamp decelleration to be less than that required to reduce the forward speed
// to zero in the next interval
acc = Math.max(0.0001 + (-aSpeed / interval), acc);
}
}
return acc;
}
UserDefinedFollowingModel
public UserDefinedFollowingModel()
freeAcceleration
public static double freeAcceleration(IMotor vehicle,
double targetSpeed,
double targetDistance,
double dt)
followingAcceleration
public static double followingAcceleration(IMotor vehicle,
IMotor leader,
double gap,
double interval)