% Controller that works by considering the bearing from current position to the goal 
% in a frame of reference with current bearing aligned with the positive X-axis. In 
% that frame of reference, a vector to the goal with a positive Y component means 
% we need to turn left; a negative Y component means we need to turn right; and a 
% very small Y component means we should go straight.
function [throttle, steering] = simple_controller(position, bearing, start, goal)
    
    throttle = 1;     % Keep your foot on the pedal!
    steering = 0; 	% No turning
    
    % Note that our steering control input is a value inversely proportional
    % to the robot's turning radius.

    % Another simple one for you to try.  Delete the % signs in front of the lines below.
%     to_goal = goal - position;
%     v = rotate(to_goal, -bearing);
%     v = v/norm(v);
%     
%     if abs(v(2)) < 0.001
%         steering = 0;
%     elseif v(2) > 0
%         % Turn left!
%         steering = -0.2;
%     else
%         % Turn right!
%         steering = 0.2;
%     end
%     throttle = 1; % Keep your foot on the pedal!    