clc
close all
clear all

%Simulation of a single link pendulum
%<<onelink.png>>

%DH for link 1 except qi
parms.a1   =  1;
parms.alpha1 = 0;
parms.d1=0;

%mass, inertia and gravity.
parms.m1  =  1;
parms.I1  =  0.5;
parms.g   =  10;

%stuff for animation
parms.time_delay = 0.1; %delay between frames, will need some fine tuning for different computers
parms.framespersec = 30;

%step size for integration. Accuracy increases as h decreases
h = 0.001;

%set the time
t0 = 0; %start time
tN = 2; %end time
N = (tN-t0)/h;
t = linspace(t0,tN,N);

%initial conditions
theta1    = 0; %initial position
theta1dot    = 0;%final position
x0=[theta1 theta1dot]';

%integrate equations of motion
x = ode4('onelink_rhs',t,x0,parms);

%do some plots
figure(1)
plot(t,x(:,1),'b');
xlabel('time');
ylabel('position');

figure(2)
plot(t,x(:,2),'r');
xlabel('time');
ylabel('velocity');

figure(3) %animation
onelink_animation(t,x(:,1),parms);