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 CalculateControls()
{
// Receive feedback of the current treadmill velocity
carController.treadmillVelCallback(track); // temporary
//temporary
CarController.CarCommand command = carController.syncCallback(new Pose(transform.position, transform.rotation), linearVel, angularVel);
//TODO - figure out how to use throttle and steer to move the car
carCommandCallback(command);
Debug.Log(string.Format("Steer after passing through sys: {0}", steer));
Debug.Log(string.Format("Throttle after passing through sys: {0}", throttle));
UpdateCar();
}
// TODO - fix this once using ROS#
void carCommandCallback(CarController.CarCommand command)
{
steer = steerSys.Output(command.steer);
throttle = throttleSys.Output(command.throttle);
}
