% motor control test
clear all
Fs = 1000 ;
t=0:1/Fs:5 ;
m_input = zeros(size(t)) ; % init motor input array
m_output = zeros(size(t)) ;% starts at zero rps
c_error = zeros(size(t)) ;% starts at zero rps

%motor model is a 2-pole low pass
motor_cutoff_freq = 2; %Hz
[b_motor, a_motor] = butter(2, motor_cutoff_freq/(Fs/2)) ;

% control input revolutions/sec
rps = 10 ;
% up-step at t=1
c_input = rps * (t>1);
% down-step at t=3
c_input(t>3) = rps/2 ;

% PID parameters
A = 8 ;
B = .02 ; pid_integral = 0;
C = 200 ;

for i = 3:length(t)
    c_error(i) = (c_input(i) - m_output(i-1)) ;
    
    % PID algorithm for a system with NO negative output
    % get integral error term
    if sign(c_error(i)) == sign(c_error(i-1))
        pid_integral = pid_integral + c_error(i) ;
    else
        pid_integral = 0;
    end
    
    % sum the PID terms and zero neg output
    m_input(i) = ...
            A * c_error(i) + ...
            B * pid_integral  + ...
            C * (c_error(i)-c_error(i-1))  ;
    m_input(i) = m_input(i) * (m_input(i)>0) ;
    
    % compute motor response to input
    m_output(i) = ...
            b_motor(1)*m_input(i) + ...
            b_motor(2)*m_input(i-1) + ...
            b_motor(3)*m_input(i-2) - ...
            a_motor(2)*m_output(i-1) - ...
            a_motor(3)*m_output(i-2) ;
end

figure(1); clf;
plot(t, c_input);
set (gca, 'ylim', [0, max(c_input)*2])
hold on
plot(t, m_output,'r');