opened_loop_pendule.cpp 5.8 KB
Newer Older
1
/* ============================================================================
Fnadi Mohamed's avatar
Fnadi Mohamed committed
2
 * D Y N I B E X - Guaranteed Identification of Viscous Friction via IA and Set-iversion
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
 * ============================================================================
 * ---------------------------------------------------------------------------- */

#include "ibex.h"
#include <math.h> 

using namespace ibex;
using namespace std; 

#define __PREC__ 1e-6     	// Précision
#define __METH__ HEUN      	// Méthode de résolution de ODE  RK4  HEUN 
#define __DURATION__ 6  	// Duréé de simulation 

int main(){

	//========================================================================
	// Déclaration des constantes (Pendule inverse + Moteur)
	//========================================================================
	Interval mp(0.08); 		       			// masse du pendule [kg]
	Interval ma(0.2);  		    			// masse du bras rotatif [kg]
	Interval M(0.01);						// masse accrochée  [kg]
	Interval la(0.1); 	     				// longueur du bras rotatif [m]
   	Interval lp(0.15);   					// longeur du pendule [m]
   	Interval r0(0.05);						// longeur par rapport à la base 
   	Interval N(5.0);  						// Rapport de réduction engrenage  
   	Interval Ja(1834.0*0.68*1e-6); 			// inertie bras rotatif [kg.m^2] 
   	Interval Jp(562.802*0.68*1e-6);		  	// inertie pendule [kg.m^2]   
   	Interval g(9.81);  						// gravité

  


   	// Moteur courant continu DCX22LG - MDP

   	Interval Ke(1.0/((981.0*2.0*M_PI)/60.0));		//  Constante de vitesse [981 tr/min/V]
   	Interval Kc(9.73e-3);  							//  Constante de couple [9.73 mNm/A]
   	Interval L(0.0346e-3);							//  Inductance [Heneri]
   	Interval f(0);									//  coeficient frottement visqueux
   	Interval Rm(0.335);								//  Résistance de l'induit (ohm)
   	Interval Jm(9.06e-7);							//  Inertie Moteur [g.cm^2] 
   	Interval mu_m(0.906);  							//  Rendement Moteur 
   	Interval Im(0.0818);         					//  Courant à vide
	Interval Cr(Kc*Im);         					//  Couple résistif à vide
	Interval Nm(5.2);								//  Rapport du réduction moteur 
	Interval Rv(34.0*1e3); 							//  Pente vitesse/couple [tr/mn/mN]
   	Interval w0(2052.0/2.0);   						//  Vitesse à vide 
   	Interval eta_m(0.9);   							//  rendement moteur

   	// Paramètres dynmaiques


   	Interval mu1(pow(lp,2)*(mp/4.0+M)+Jp);					// Dynamic parameter
   	Interval mu2(pow(la,2)*(mp+M)+Jm/(pow(N,2))+Ja);		// Dynamic parameter
   	Interval mu3(lp*la*(mp/2+M));							// Dynamic parameter
   	Interval mu4(pow(lp,2)*(mp/4.0+M)+Jp);					// Dynamic parameter
   	Interval mug(lp*g*(mp/2+M));


   	Interval al(0.0033472);						// Dynamic parameter
   	Interval be(0.0038852);						// Dynamic parameter
   	Interval ga(0.0024879);						// Dynamic parameter
   	Interval del(0.097625);						// Dynamic parameter

   	cout << "mu1 = " << mu1 << "\t mu2 = " << mu2 << "\t mu3 = " << mu3 << "\t mu4 = " << mu4 << "\t mug = " << mug <<endl; 

   	Interval fv1(0.16);  						// Viscous Colomb friction coefficient  joint 1  0.07619 (50 deg : IA : 0.13  LSMI : 0.06) 0.085
   	Interval fc1(0.0);  						// Static Colomb friction coefficient  joint 1
   	Interval fv2(0.0006);  						// Viscous Colomb friction coefficient  joint 2  (50 deg : IA: 0.00052  LSMI : 0.000382)  0.00043
   	Interval fc2(0.000);  						// Static Colomb friction coefficient  joint 2



   	//========================================================================
	// Relation between velocity/torque of the motor (Inputs) ((n0-n)/(Rv*Nm))
	//========================================================================
	Interval w(0.170);  											// Vitesse moteur souhaitée rad/s
   	Interval GAMMA1(-(N*w0*(1-w))/(Rv*eta_m));  					// Troque of the joint 1 including friction
   	cout <<"Couple moteur en N.m : \t" << GAMMA1 <<endl;
   	Interval GAMMA2(-0.0,0.);  						     			// Troque of the joint 2 including friction


	const int n= 4;
	Variable y(n);
	IntervalVector yinit(n);
	yinit[0] = Interval(13.4*(M_PI/180.0));
	yinit[1] = Interval(0.0);
	yinit[2] = Interval(-79.25*(M_PI/180.0));
	yinit[3] = Interval(0.0);

	//=============================================================================
	// Dynamique du pendule inverse avec incertitude sur ses paramètres
	//=============================================================================

	Function ydot = Function(y,Return( y[1],
				    (1/(-pow(mu3,2)*pow(cos(y[2]),2)+mu1*mu4*pow(sin(y[2]),2)+mu2*mu4))*(-mu3*cos(y[2])*(mu1*sin(y[2])*cos(y[2])*pow(y[1],2)+GAMMA2 - fv2*y[3]-mug*sin(y[2]))+mu4*(mu3*sin(y[2])*pow(y[3],2)-mu1*sin(2*y[2])*y[1]*y[3]+GAMMA1 - fv1*y[1])), 
				    y[3],
				    (1/(-pow(mu3,2)*pow(cos(y[2]),2)+mu1*mu4*pow(sin(y[2]),2)+mu2*mu4))*((mu1*pow(sin(y[2]),2)+mu2)*(mu1*cos(y[2])*sin(y[2])*pow(y[1],2)+GAMMA2-fv2*y[3]-mug*sin(y[2]))-mu3*cos(y[2])*(mu3*sin(y[2])*pow(y[3],2)-2*mu1*cos(y[2])*sin(y[2])*y[1]*y[3]+GAMMA1 -fv1*y[1]))
				    ));

	// Function ydot = Function(y,Return( y[1],
	// 			    (1/(al*be-pow(ga,2)+(pow(be,2)+pow(ga,2))*pow(sin(y[2]),2)))*(be*ga*(pow(sin(y[2]),2)-1)*sin(y[2])*y[1]-2*pow(be,2)*cos(y[2])*sin(y[2])*y[3]+be*ga*sin(y[2])*pow(y[3],2)-ga*del*cos(y[2])*sin(y[2])+be*GAMMA1-ga*cos(y[2])*GAMMA2), 
	// 			    y[3],
	// 			    (1/(al*be-pow(ga,2)+(pow(be,2)+pow(ga,2))*pow(sin(y[2]),2)))*(be*(al+be*pow(sin(y[2]),2))*cos(y[2])*sin(y[2])*pow(y[1],2)-2*be*ga*(1-pow(sin(y[2]),2))*sin(y[2])*y[1]*y[3]-pow(ga,2)*cos(y[2])*sin(y[2])*y[3]+del*(al+be*pow(sin(y[2]),2))*sin(y[2])-ga*cos(y[2])*GAMMA1+(al+be*pow(sin(y[2]),2))*GAMMA1)
	// 			    ));

	ivp_ode problem = ivp_ode(ydot,0.0,yinit);


	// Integration numerique ensembliste
	simulation simu = simulation(&problem,__DURATION__, __METH__, __PREC__);
	simu.run_simulation();

	//For an export in order to plot
	simu.export_y0("/home/mohamed/Desktop/NMPC_Pendule/export-q1.txt");
	simu.export1d_yn("/home/mohamed/Desktop/NMPC_Pendule/export-opend-loop-q2.txt",2);


	return 0;

}