% VEX robot simulator

clear
R = 1;        % armature circuit resistance, ohms
Kb = 1;       % rotor back voltage constant, v/rad/s
L = 1;        % armature circuit inductance, henrys
Km = 1;       % motor torque constant,nm/a
B = 1;        % rolling resistance coefficient, n/rad/s
J = 1;        % wheel and rotot moment of inertia, kgmm

m = 2;         % mass of robot, kg 
r = .02;       % robot wheel radius, m

% initial values for state variables
I(1) = 0;        % current, a 
omega(1)=0;       % rotor angle, r
theta(1)=0;       % rotor angular rate, rad/s

t(1)=0;
n = 1500;
dt = 0.01;

va=zeros(1,n+1);  % program a 1 second unit pulse at t = 2
for i=201:300
    va(i)=1;
end

for i=1:n                
    t(i+1)=t(i)+dt;
    I(i+1) = I(i) + dt*(va(i) - R*I(i) - Kb*omega(i))/L;
    theta(i+1) = theta(i) + dt*omega(i);
    omega(i+1) = omega(i) + dt*(Km*I(i) - B*omega(i))/(J + m*r^2);
end

figure(1)    % graph state variables
plot(t, theta)
hold on
plot(t, I, 'red')
plot(t, va, 'cyan')
plot(t,omega*Kb, 'green')
plot(t, va - I*R - omega*Kb,'magenta')
plot(t,(Km*I - B*omega)/(J + m*r^2),'black')
grid
xlabel('t')
title({'VEX robot response to unit pulse','va - cyan, I - red, omega prime - black, omega - green, theta - blue, vL - magenta'})
hold off




    
    

