%  2link arm controller simulator
%  
% Hiroaki Gomi 21/July/2000
%

close all
clear all
Fig1 = figure;
Fig2 = figure;
Fig3 = figure;
%set(Fig1,'Position',[100 680 350 250])
%set(Fig2,'Position',[100 300 700 300])
%set(Fig3,'Position',[450 680 350 250])
set(Fig1,'Position',[300 250 250 250])
set(Fig2,'Position',[300 50 500 200])
set(Fig3,'Position',[550 250 250 250])

global L1;
global L2;
global th_r;
global th_h;
global th_s;
global tau;

% feedback gain for [th1_err, th2_err, th1d_err, th2d_err]
FbGain = [170, 0, 10, 0; 0, 200, 0, 20];
%FbGain = [2.5,0,0.7,0; 0,2.5,0,0.7];
%FbGain = [8,0,2,0; 0,10,0,2.5];


sw_smith = 1  % smith compensator switch
delay_t = 0.060; % sec   system observation delay
delayP_t = 0.060; % sec  estimated delay 

% arm length
L1 = 0.282;
L2 = 0.303;

%if 0   %
  Z1_sim = 0.327;
  Z2_sim = 0.109;
  Z3_sim = 0.106;

%else   % feedforward controller output is zero if Z1,2,3_sim are
       % zero.
%  Z1_sim = 0;
%  Z2_sim = 0;
%  Z3_sim = 0;
%end


for i = 1:10
 pos_specify
 armSim
 sw_redraw = 1;
 while  sw_redraw == 1 
   anim
   redraw = input('animation redraw (y/n)? ', 's')
   if redraw ~= 'y'
     sw_redraw = 0;
   end
 end
end