%% Steering Angle Command for Vehicle in Reverse 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 reverse 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 (5 m, 9 m) and has an orientation angle of 90 degrees.

refPose = [5, 9, 90]; % [meters, meters, degrees]

% Set the current pose of the vehicle. The pose is at position (5 m, 10 m) and has an orientation angle of 75 degrees.

currPose = [5, 10, 75]; % [meters, meters, degrees]

% Set the current velocity of the vehicle to –2 meters per second. Because the vehicle is in reverse motion, the velocity must be negative.

currVelocity = -2; % meters per second

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

steerCmd = lateralControllerStanley(refPose,currPose,currVelocity,'Direction',-1)

% Copyright 2018 The MathWorks, Inc.