function PS1
% 16299 Fall 2021
% PS1 Supplement: Alternate PID exploration function for those who can't 
% get Mujoco to work
%
% Code to apply open and closed loop controls should be inserted at line 31
% An example applying an open loop pulse is provided

x = [0;0];   % initial condition, the state x = [ y, y_dot ]'
y(1) = x(1); % inital output
t = 0;       % inital time
T = 0.002;   % simulation time step
tf = 10;     % final time
u = 0;       % initial value of input
    
num_steps = ceil(tf/T);

% begin simulation loop
for i = 1:num_steps
    
    % use RK4 to get next state and compute next output
    % This is like reading the sensor inputs from the system
    k1 = f(x(:,i),u)*T;
    k2 = f(x(:,i)+k1/2,u)*T;
    k3 = f(x(:,i)+k2/2,u)*T;
    k4 = f(x(:,i)+k3,u)*T;
    x(:,i+1) = x(:,i) + (k1 + 2*k2 + 2*k3 + k4)/6;
    t(i+1) = t(i) + T;
    y(i+1) = x(1,i); % compute next output
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   Write your code to determine control input below   %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % apply an open loop pulse
    u_ref = 0;
    if i <= 50 
        u = 8.5;
    else
        u = 0;
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   End of code to determine control input             %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     
    
    % Normally, you would send the control input to the system here,
    % but you don't need to do that since this is just a MATALB simulation

end

% plot output
figure(1)
hold off
plot(y,'b')
hold on
plot([1 5000], [u_ref u_ref],'r--')
xlabel('simulation time steps')
ylabel('y(t)')
title(['Position vs. Time Step']) 
grid


function x_dot = f(x,u)
% state equation for mass-spring example

m = .6;
k = 0;
gamma = 0.5;

% mass spring damper:
x_dot = [x(2); 1/m*(u - k*x(1) - gamma*x(2))];

% simple motor position
%x_dot = [x(2); 1/m*(u-gamma*x(2))];
    