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); } }