%% Steering Angle Command for Vehicle in Forward Motion
% Compute the steering angle command that adjusts the current pose of a vehicle to a reference pose along a driving path. The vehicle is in forward motion.
% In this example, you compute a single steering angle command. In path-following algorithms, compute the steering angle continuously as the pose and velocity of the vehicle change.
%
% Set a reference pose on the path. The pose is at position (4.8 m, 6.5 m) and has an orientation angle of 2 degrees.

refPose = [4.8, 6.5, 2]; % [meters, meters, degrees]

%
% Set the current pose of the vehicle. The pose is at position (2 m, 6.5 m) and has an orientation angle of 0 degrees. Set the current velocity of the vehicle to 2 meters per second.

currPose = [2, 6.5, 0]; % [meters, meters, degrees]
currVelocity = 2; % meters per second

%
% Compute the steering angle command. For the vehicle to match the reference pose, the steering wheel must turn 2 degrees counterclockwise.

steerCmd = lateralControllerStanley(refPose,currPose,currVelocity)

% Copyright 2018 The MathWorks, Inc.