function [x,y,theta,time] = robobrainControl(x0,y0,theta0,delta,N,kp,kd,dsep)

%%  William Dunbar and Jorge Cortes

%%  Computer Engineering
%%  University of California at Santa Cruz
%%  dunbar@soe.ucsc.edu

%%  Applied Mathematics and Statistics
%%  University of California at Santa Cruz
%%  jcortes@soe.ucsc.edu
	
%%  Copyright (c) July 2005, Jorge Cortes, William Dunbar
%%  Permission is granted to copy, modify and redistribute this file, 
%%  provided that this header message is retained.

% Function robobrainControl(x0,y0,th0,delta,N,kp,kd,dsep) 
% has input arguments:
%   x0   - initial x
%   y0   - initial y
%   th0  - initial theta
%   kp   - proportional control gain
%   kd   - derivative control gain
%   dsep - desired separation distance (constant) between robot and wall
%
% Function outputs visualization of feedback control.
% 
% Assumes that the distance to the wall can be measured 
% with precision in real time. In this example, x is the distance to the
% wall, for system model:
%   
%   x(k+1) = x(k) + delta*u(k)*cos(theta(k));
%   y(k+1) = y(k) + delta*u(k)*sin(theta(k));
%   theta(k+1) = theta(k) + delta*v(k);
%
% Controller is:
%   u(k) = vnom;
%   v(k) = kp(x(k) - dsep) + kd(x(k) - x(k-1))/delta;
%
%

x(1) = max(0,x0);
y(1) = y0;
theta(1) = theta0;
time(1) = 0;

%max forward speed is (7.7/2)*100*2*pi/60 - go with 75rpm
umax = (7.7/2)*75*2*pi/60;

vnom = 1.0;
vk = kp*(x(1) - dsep) + kd*(x(1) - x(1))/delta;
% max spin rate is 75rpm*2pi/60
vk = min(vk,75*2*pi/60);
vk = max(vk,-75*2*pi/60);

for k=1:N,

    x(k+1) = x(k) + delta*vnom*cos(theta(k));
    x(k+1) = max(0,x(k+1));
    y(k+1) = y(k) + delta*vnom*sin(theta(k));
    theta(k+1) = theta(k) + delta*vk;
    time(k+1) = time(k) + delta;
    vk = kp*(x(k+1) - dsep) + kd*(x(k+1) - x(k))/delta;
    vk = min(vk,100*2*pi/60);
    vk = max(vk,-100*2*pi/60);
end

figure(1),plot(x,y),grid on
figure(2),plot(time,theta),grid on
