void UpdateCar()
{
//Get updated track velocity
float dt = (1 / Constants.targetHz);
float trackVel = track.gameObject.GetComponent <TrackSpeedSubscriber>().GetVelocity();
//Calculate car controls
Pose convertedCoordinatesPose = new Pose(new Vector3(transform.position.z, transform.position.x, 0), transform.rotation);
Vector2 convertedCoordinatesLinearVel = new Vector2(linearVel.z, linearVel.x);
CarController.CarCommand command = carController.calculateControls(convertedCoordinatesPose, convertedCoordinatesLinearVel, angularVel, trackVel);
steer = steerSys.Output(command.steer);
throttle = throttleSys.Output(command.throttle);
//Update bicycle model
carState.UpdateState(throttle, steer, dt, trackVel);
//Update the state of the car sprite
transform.position = new Vector3(carState.x, transform.position.y, carState.z);
float newAngle = (carState.psi * Mathf.Rad2Deg);
Quaternion rotation = Quaternion.Euler(0, (carState.psi * Mathf.Rad2Deg), 0);
transform.rotation = rotation;
linearVel = new Vector3(carState.dx, 0, carState.dz);
angularVel = carState.dv;
}
void Update()
{
CheckNullSetDefaultState();
string resultStateName = currentState.UpdateState();
SetStateByName(resultStateName);
}
void UpdateCar()
{
float dt = (1 / Constants.targetHz);
float trackVel = track.gameObject.GetComponent <TrackSim>().vel;
//Update bicycle model
carState.UpdateState(throttle, steer, dt, trackVel);
//Update the state of the car sprite
transform.position = new Vector3(carState.x, carState.y);
float newAngle = (carState.psi * Mathf.Rad2Deg);// + Constants.neutralHeading;
Debug.Log(string.Format("newAngle: {0}", newAngle));
Quaternion rotation = Quaternion.Euler(0, 0, (carState.psi * Mathf.Rad2Deg));// + Constants.neutralHeading);
Debug.Log(string.Format("rotation quaternion: {0}", rotation));
transform.rotation = rotation;
linearVel = new Vector3(carState.dx, carState.dy, 0);
angularVel = carState.dv;
Debug.Log(string.Format("Linear vel X: {0}, Y {1}", linearVel.x, linearVel.y));
}
