CONTINUOUS SYSTEM ROB2GL

" States, derivates and time:

STATE q1 q11 q2 q22
DER dq1 dq1s dq2 dq2s
TIME t
" Initializations:
q1:0.2
q2:0.2
" Equations:
dq1=q11
dq1s=(Tao1*h-T2*b+b*i+b*j+d*h-e*h-f*h-c*h)/o1
dq2=q22
dq2s=(Tao2/h)-(g1/h)*(o3/o2)
o3=Tao1*h-Tao2*b+b*i+b*j+d*h-e*h-f*h+c*h
o2=(a*h-b*g1)-(i/h)-(j/h)
o1=(a*h-b*g1)
a=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+2*m2*l1*lc2*cos(q2)+I1+I2
b=m2*lc2*lc2+m2*l1*lc2*cos(q2)*I2
c=2*m2*l1*lc2*sin(q2)*dq1*dq2
d=m2*l1*lc2*sin(q2)*dq2*dq2
e=(m1*lc1+m2*l1)*g*sin(q1)
f=m2*g*lc2*sin(q1+q2)
g1=m2*lc2*lc2+m2*l1*lc2*cos(q2)+I2
h=m2*lc2*lc2+I2
i=m2*l1*lc2*dq1*dq1*sin(q2)
j=m2*g*lc2*sin(q1+q2)
" Controlador
Tao1=a*v1+b*v2-c-d+j+e+f
Tao2=g1*v1+h*v2+i+j
v1=q1sdd-k21*(q11-q1sd)-k11*(q1-q1s)
v2=q2sdd-k22*(q22-q2sd)-k12*(q2-q2s)
k21=2*z1*wn1
k11=wn1*wn1
k22=2*z2*wn2
k12=wn2*wn2
z1=1
z2=1.5
wn1=1
wn2=1.5
q1s=w*t
q1sd=w
q1sdd=0
w:1
"trajectory planning

q2s=fs
q2sd=fsd
q2sdd=fsdd
t1:0
t2:27
delt= t2-t1
tdif = abs(t-t1)
z1f = 1.5707
z1in = 0
z1dif =z1f-z1in
mul5
mul4
mul3
mul2
mul1

=
=
=
=
=

tdif*mul4
tdif*mul3
tdif*mul2
tdif*mul1
tdif

div9
div8
div7
div6
div5
div4
div3
div2
div1

=
=
=
=
=
=
=
=
=

delt*div8
delt*div7
delt*div6
delt*div5
delt*div4
delt*div3
delt*div2
delt*div1
delt

r1:
r2:
r3:
r4:
r5:
r6:

252
1050
1800
1575
700
126

z1st1 = z1in
z1stt = z1in+z1dif*mul5/div5*fr
fr=(r1-r2*mul1/div1+r3*mul2/div2-r4*mul3/div3+r5*mul4/div4-r6*mul5/div5)
z1st2 = z1f
z1std1 = 0
z1stdt= z1dif*mul4/div4*frd
frd =(5*r1/div1-6*r2*mul1/div2+frd2)
frd2=(7*r3*mul2/div3-8*r4*mul3/div4+9*r5*mul4/div5+frd3)
frd3 = -10*r6*mul5/div6
z1std2 = 0
z1stdd1 = 0
z1stddt = z1dif*mul3/div3*frdd
frdd= 20*r1/div2-30*r2*mul1/div3+ frdd2
frdd2 = 42*r3*mul2/div4-56*r4*mul3/div5+72*r5*mul4/div6+frdd3
frdd3 = -90*r6*mul5/div7
z1stdd2 = 0
fs = IF t < t1 THEN z1st1 ELSE (IF t < t2 THEN z1stt ELSE z1st2)
fsd = IF t < t1 THEN z1std1 ELSE (IF t < t2 THEN z1stdt ELSE z1std2)
fsdd = IF t < t1 THEN z1stdd1 ELSE (IF t < t2 THEN z1stddt ELSE z1stdd2)

"desnormalizacion
l2:0.5
l1:1
m2:0.5
m1:0.5
lc1:0.5
lc2:0.25
I1:5
I2:10
g:9.81
END