%% estimation_inertie_bras.m %%
% Programme Matlab permettant le calcul des paramètres d'asservissement du
% moteur de l'épaule droite (RShoulderpitch) lors d'un mouvement de
% flexion.

%% Initialisation du programme
clear all
clc
close all

%% Apport de chacun des membres en g.mm2
Iyy_RightShoulder=14057.99;
Iyy_RightBicep=290014.45;
Iyy_RightElbow=62254.05+(-25.60+105)^2*59.71;
Iyy_RightForeArm=88903.15+(25.56+105)^2*77.24;
Iyy_RightHand=279473.51+(31.80+105+55.95)^2*166.53;

%% Inertie totale du bras en en kg.m2
Jbras=(Iyy_RightShoulder+Iyy_RightBicep+Iyy_RightElbow+Iyy_RightForeArm+Iyy_RightHand)*1e-9;
%% Inertie du rotor
Jrotor=0.852*1e-7; %g.cm2
%% Inertie totale du bras en en kg.m2 ramené sur l'arbre moteur
J=Jrotor+Jbras/(150.27)^2;

%% Paramètres moteurs et coefficient de frottement
R=39.7;
ke=21.4e-3;
kt=20.1e-3;
mu=6e-6;
%% Constante de temps mécanique (voir partie préparatoire)
tm=R*J/(ke*kt+R*mu);

%% Gain optimum pour un amortissement de 0.7 (voir partie préparatoire)
K=1/(4*0.7^2*tm);

%% Affichage des résultats
disp(['Inertie du bras ramenée à l''arbre moteur: J=',num2str(J),'kg.m2'])
disp(['Constante de temps mécanique: tm=',num2str(tm*1000),'ms'])
disp(['Gain optimum: K=',num2str(K),'s-1'])

