%
% PD control example (draft) : 1 DOF angular servo with hacky dynamics
% 
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: February 4, 2008
%
% based on pseudocode example from Wikipedia PID control entry 
%

spring = 1;

dt = 0.01;
if (spring == 1) % lightly stiff spring
    kp = 0.1;
    kd = 0;
elseif (spring == 2) % stiffer spring, slightly damped
    kp = 1.1;
    kd = 0.000001;
elseif (spring == 3) % stiffer spring, a little too much damping
    kp = 1.1;
    kd = 0.002;
elseif (spring == 4) % overstiff spring that overoscillates
    kp = 1.9;
    kd = 0.05;
end

xdesired = 3*pi/4; % desired view angle
xcurrent = 1*pi/12; % starting view angle
t = 0; % time initialized at zero
previous_error = 0;
end_t = 1;

subplot(1,2,1);
plot([t end_t],[xdesired xdesired],'g-','LineWidth',3);
hold on;
plot([t],[xcurrent],'r.','MarkerSize',25);

subplot(1,2,2);
plot([t cos(xcurrent)],[t sin(xcurrent)],'r.-','LineWidth',5);    


while (t < end_t-dt) % loop for 1 simulation second
    
    % PD control without integral term
    xerror = xdesired - xcurrent;
    %integral = integral + error*dt % assume no steady state error
    error_derivative = (xerror - previous_error)/dt;
    xcommand = kp*xerror + kd*error_derivative; %+ Ki*integral  % integral term is removed
    xprevious = xcurrent;
    xcurrent = xcurrent + 1.3*rand*xcommand; % coarse model of plant dynamics with noise (definitely not physically correct, but you should get the point anyway)
    xcurrent = xcurrent + 1.0*rand*(xcurrent-xprevious);
    previous_error = xerror;
    
    t = t + dt; % update time
    
    % draw position over time
    subplot(1,2,1);
    plot([t-dt t],[xprevious xcurrent],'b.-');
    
    % draw current and desired viewing angles
    subplot(1,2,2);
    hold on;
    plot([0 cos(xcurrent)],[0 sin(xcurrent)],'b.-','LineWidth',2);    
    %hold on;
    plot([0 cos(xdesired)],[0 sin(xdesired)],'g.-','LineWidth',5);
    axis('equal');
    %hold off;
    refresh;
    drawnow;

end