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; }
// TODO - fix this once using ROS# void carCommandCallback(CarController.CarCommand command) { steer = steerSys.Output(command.steer); throttle = throttleSys.Output(command.throttle); }
public float Output(float input) { FirstOrderSystem selected = Select(input); return(selected.Output(input)); }