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