No BSD License  

Highlights from
Gantry robot simulation

image thumbnail

Gantry robot simulation

by

 

11 Oct 2007 (Updated )

Supervisory and low level robot tracking control of a 5-bar or articulated gantry using desired posi

gantry_5bar_init.m
% for use with new coord's (011005) and cm

% John Kluza
% 10/17/04
% initialization file for gantry robot demo

%  length in m
%  mass in kg
%  rotational interia in kg*m^2
%  angles in degrees

clear

%% code for time invariant reduced order predictive kalman filter
% physical params
b=.0001; % arb guess
m=.01;  % kg guess
Ts=.02; % should be same as controller sample rate

% 4 states
% continuous plant
A_c=[0 1 0 0; 0 -b/m 0 0; 0 0 0 1; 0 0 0 -b/m];
B_c=[0 0 0 0; 1/m 0 1/m 0; 0 0 0 0; 0 1/m 0 1/m]; %last 2 inputs for process noise
C_c=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
D_c=[0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0];

sys_c=ss(A_c,B_c,C_c,D_c);

% convert to discrete plant
sys_d=c2d(sys_c,Ts);

% define noise covariance matrices
Q=5*eye(2); %process noise - guessing more process noise than sensor noise, due to highly simplified plant
R=1*eye(2); %measurement noise
N=0; %p*m noise - correlation between process and sensor noise???

% find kalman estimator
[kest,L,P,M,Z] = kalman(sys_d,Q,R,N, [1 3], [1 2]);

[A_k,B_k,C_k,D_k]=ssdata(kest);


%% unit conversions
in2cm=2.54;
%oz2kg=2.2/16;
r2d=180/pi;

%% physical parameters
% 10/17/04 L and M from Quanser manual page 6 (SRV02_Exp13_2DOF_SPG.pdf)
L=5*in2cm;  % .127m
Lab=L;  % .127m
Lbc=L;
Lcd=L;
Lde=L;
Lae=2*L; % .254m

Mab=65; %g
Mbc=65;
Mcd=65;
Mde=65;

% % J calc's off of formulas at:
% % http://hyperphysics.phy-astr.gsu.edu/hbase/mi2.html
% % http://hyperphysics.phy-astr.gsu.edu/hbase/mi.html#cmi
R=1.5; % 1.5 centimeter radius for each arm (guess)
Jab=[(Mab*R^2)/2 0 0; 0 (Mab*Lab^2)/12 0; 0 0 (Mab*Lab^2)/12]; 
Jbc=[(Mbc*R^2)/2 0 0; 0 (Mbc*Lbc^2)/12 0; 0 0 (Mbc*Lbc^2)/12]; 
Jcd=[(Mcd*R^2)/2 0 0; 0 (Mcd*Lcd^2)/12 0; 0 0 (Mcd*Lcd^2)/12]; 
Jde=[(Mde*R^2)/2 0 0; 0 (Mde*Lde^2)/12 0; 0 0 (Mde*Lde^2)/12]; 

ang=90; % original arm angles (all right angles)

% friction (guesses in arbitrary units) 10/17/04
Ba=-.0005; 
Bb=0; % not implemented in model
Bc=0; % not implemented in model
Bd=0; % not implemented in model
Be=-.0005; 

%% controller values
Ts=.02; %

Kpax=-.1; %-2 2
%Kpay=0;
%Kpex=0;
Kpey=-.1; %-2 -2
Kdax=.0015; %0 .1 -.1
Kdey=.0015; %0 .1 .1

err_tol=.6; %max shortest line error between end effector and target

T_limit=.1; % max Torque input, in pos and neg directions
% 
% %% video track rejector values
% approx_deriv=tf([1 0],[.01 1]);
% approx_deriv_d=c2d(approx_deriv, Ts);
% [num_d,den_d]=tfdata(approx_deriv_d);
% num_d=num_d{1};
% den_d=den_d{1};


%% initial x and y endpoint positions
endpoint_x_orig=L;
endpoint_y_orig=10.3; % .1 cm above bottom of display

%% VIP params
min_area=2000;
max_area=5500;
num_objects=3;


%% motor model param's (starting with Craig's robot numbers from sp1)
shaftInertia = 2.72*10^-6;
backEMF = .2096;
torqueConstant = .06338;
armatureResistance = 1.906;
armatureInductance = .0005406;
dampingCoeff = .01904;

% video parameters
%exists separately in init and output in msfun, also in gantry init
videoRes=[240 320];
videoArea=[7.5 10]; %length of area video covers in m's

%% load robot arm param's from mat's
load coordConvertParams
load speend
load sroend

Contact us