void UpdateBrakingForIntersection(Collider other)
{
try
{
IntersectionLightSingleton intersection = other.transform.gameObject.GetComponentInChildren <IntersectionLightSingleton>();
TargetNavigation tv = GetComponent <TargetNavigation>();
TurnType turnType = tv.CurrentTrajectory.Turn;
IntersectionRoad road = tv.CurrentTrajectory.Road;
LightPhase phase = (road == IntersectionRoad.Road1) ? intersection.phase.road1 : intersection.phase.road2;
switch (phase)
{
case LightPhase.Green:
_brakingForRedLight = false;
break;
case LightPhase.Yellow:
_brakingForRedLight = true;
break;
case LightPhase.Red:
_brakingForRedLight = true;
break;
}
Debug.Log("Vehicle performing " + turnType.ToString() + " movement on " + road.ToString() + " has entered IntersectionInteriorVolume when light is " + phase.ToString());
}
catch (NullReferenceException ex)
{
Debug.Log(ex.Message);
}
return;
}
public void Update()
{
//Get state relative to target
try{
TargetNavigation tv = GetComponent <TargetNavigation>();
Transform target = GetComponent <TargetNavigation>().CurrentTarget;
Rigidbody carbody = GetComponent <Rigidbody>();
//Calculate control error
var angle1 = Mathf.Atan2(transform.forward.z, transform.forward.x);
var dir = target.position - transform.position;
var angle2 = Mathf.Atan2(dir.z, dir.x);
var angleDiff = angle2 - angle1;
//Calculate control outputs
float steering = -steeringGain *Mathf.Sin(angleDiff) * maxSteeringAngle;
var motor = CalculateMotorTorqueSignal(carbody);
var brake = CalculateBrakeTorqueSignal(carbody);
//Implement control signals
foreach (AxleInfo axleInfo in axleInfos)
{
if (axleInfo.steering)
{
axleInfo.leftWheel.steerAngle = steering;
axleInfo.rightWheel.steerAngle = steering;
}
if (axleInfo.motor)
{
axleInfo.leftWheel.motorTorque = motor;
axleInfo.rightWheel.motorTorque = motor;
axleInfo.leftWheel.brakeTorque = brake;
axleInfo.rightWheel.brakeTorque = brake;
}
}
//Log state
AppendState(carbody);
WriteIncrementalStateHistory();
var lifetime = DateTime.Now - _startTime;
if (lifetime >= TimeSpan.FromSeconds(maxLifetimeSeconds))
{
Destroy(this.gameObject);
}
}
catch (ArgumentOutOfRangeException ex) {
Console.WriteLine(ex.Message);
}
}
