% Jerry Pratt
% March 31, 2000
% Plotting speed variation for PhD thesis with Flamingo...


load down_lh
load gc_lheel
load gc_ltoe 
load gc_rheel
load gc_rtoe
load left_state
load lshin_d
load lshind_d
load lshindd_d
load lthigh_d
load lthighd_d
load lthighdd_d
load q_la
load q_lh
load q_lk
load q_lshin
load q_lthigh
load q_ra
load q_rh
load q_rk
load q_theta
load q_z
load qd_la
load qd_lh
load qd_lk
load qd_lshin
load qd_lthigh
load right_state
load t
load tau_la
load tau_lh
load tau_lk
load tau_ra
load tau_rh
load tau_rk
load transfer_ratio
load v_nom
load vel


figure(1);clf;
grid on;
hold on;
plot(vel);
hold off;

%x_begin=input('enter x_begin  ')
%x_end = input('enter x_end  ')

x_begin = 250;
x_end = 1000;



down_lh = down_lh(x_begin:x_end);
gc_lheel = gc_lheel(x_begin:x_end);
gc_ltoe = gc_ltoe (x_begin:x_end);
gc_rheel = gc_rheel(x_begin:x_end);
gc_rtoe = gc_rtoe(x_begin:x_end);
left_state = left_state(x_begin:x_end);
lshin_d = lshin_d(x_begin:x_end);
lshind_d = lshind_d(x_begin:x_end);
lshindd_d = lshindd_d(x_begin:x_end);
lthigh_d = lthigh_d(x_begin:x_end);
lthighd_d = lthighd_d(x_begin:x_end);
lthighdd_d = lthighdd_d(x_begin:x_end);
q_la = q_la(x_begin:x_end);
q_lh = q_lh(x_begin:x_end);
q_lk = q_lk(x_begin:x_end);
q_lshin = q_lshin(x_begin:x_end);
q_lthigh = q_lthigh(x_begin:x_end);
q_ra = q_ra(x_begin:x_end);
q_rh = q_rh(x_begin:x_end);
q_rk = q_rk(x_begin:x_end);
q_theta = q_theta(x_begin:x_end);
q_z = q_z(x_begin:x_end);
qd_la = qd_la(x_begin:x_end);
qd_lh = qd_lh(x_begin:x_end);
qd_lk = qd_lk(x_begin:x_end);
qd_lshin = qd_lshin(x_begin:x_end);
qd_lthigh = qd_lthigh(x_begin:x_end);
right_state = right_state(x_begin:x_end);
t = t(x_begin:x_end);
tau_la = tau_la(x_begin:x_end);
tau_lh = tau_lh(x_begin:x_end);
tau_lk = tau_lk(x_begin:x_end);
tau_ra = tau_ra(x_begin:x_end);
tau_rh = tau_rh(x_begin:x_end);
tau_rk = tau_rk(x_begin:x_end);
transfer_ratio = transfer_ratio(x_begin:x_end);
v_nom = v_nom(x_begin:x_end);
vel = vel(x_begin:x_end);
t = t - t(1);

lh_pow = qd_lh .* tau_lh;
lk_pow = qd_lk .* tau_lk;
la_pow = qd_la .* tau_la;

q_rthigh = q_theta + q_rh;
q_rshin = q_rthigh + q_rk;

q_x = zeros(x_end-x_begin+1,1);

distance = 0.0;
for(i=1:x_end-x_begin+1)
distance = distance - vel(i) * 0.02;
q_x(i) = distance;
end;


add_state_three;