% Example PID control 
% Written by Carrick Detweiler 2010


clear;

Kp = .1;
Ki = 0.002;
Kd = 0.5;

% The target we are trying to get to
target = 20.0;

% We are working against a spring, with the specified rest location and
% spring constant
springRest = 10.0;
springConstant = 0.003;
% Mass of the system
mass = 1.0;
% Damping factor
damping = .1;

% The maximum and minimum force we can apply
minCommand = -1;
maxCommand =  1;

% Time time window we will operate over
time = [1:200]';
% Vector to store the controller output, inited to zero
output = zeros(size(time,1),1);

% Place to store the old values
error = zeros(size(time,1),1);
derivative = zeros(size(time,1),1);
integral = zeros(size(time,1),1);

% Place to store our command
commandRaw = zeros(size(time,1),1);
command = zeros(size(time,1),1);


velocity = 0.0;

% Control loop
for t = time(2):time(end)
    % Find the current error
    error(t) = target - output(t-1);
    % Calculate the derivative
    derivative(t) = error(t) - error(t-1);
    % Update the integral
    integral(t) = integral(t-1) + error(t);

    
    %Apply the PID control and store the rusult in our current output
    commandRaw(t) = error(t)*Kp + integral(t)*Ki + derivative(t)*Kd;
    %Bound the command (maximum force we can apply)
    command(t) = min(max(commandRaw(t),minCommand),maxCommand);

    % Compute the total force (command + spring force - velocity damping (friction))
    force = (command(t)+(springRest-output(t-1))*springConstant-damping*velocity)
    % Now get the accel and velocity
    accel = force/mass;
    velocity = velocity + accel;
    % Finally move the output based on the velocity
    output(t) = output(t-1) + velocity;
end


%Plot 
figure(1);
plot(output,'LineWidth',2);
legend('Output');

figure(2);
plot([error*Kp,integral*Ki,derivative*Kd],'LineWidth',2);
legend('Error','Integral','Derivative');

figure(3);
plot([commandRaw,command],'LineWidth',2);
legend('Command Raw','Command');