-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcart_pole.py
executable file
·121 lines (110 loc) · 3.97 KB
/
cart_pole.py
1
2
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
#!/usr/bin/env python3
from tkinter import *
from math import *
import random
# procédure générale de déplacement :
def avance(gd, hb):
global x1, y1, xrect1, yrect1, widthrect1, heightrect1
x1, y1 = x1 +gd, y1 +hb
can1.coords(oval1, x1, y1, x1+30, y1+30)
can1.coords(line2, x1+15,y1+15,xrect1+(widthrect1/2),yrect1)
def avance_all_cartesian(x, theta, length):
theta -= pi/2
global x1, y1, xrect1, yrect1, widthrect1, heightrect1
xrect1 += x
can1.coords(rect1,xrect1, yrect1, xrect1+widthrect1, yrect1+heightrect1)
x1 = (xrect1 + widthrect1/2)+80*cos(theta)
y1 = (yrect1)+80*sin(theta)
can1.coords(oval1, x1-15, y1-15, x1+15, y1+15)
can1.coords(line2, x1,y1,xrect1+(widthrect1/2),yrect1)
def avance_cart(gd, hb):
global x1, y1, xrect1, yrect1, widthrect1, heightrect1
xrect1, yrect1 = xrect1 +gd, yrect1 +hb
can1.coords(rect1,xrect1, yrect1, xrect1+widthrect1, yrect1+heightrect1)
can1.coords(line2, x1+15,y1+15,xrect1+(widthrect1/2),yrect1)
def show_info():
global x, x_dot, theta, theta_dot, action
print('cart deplacement : ',x,' stick angle : ',theta,'action',action,'\n')
#print('x ball : ',x1+15,' y ball : ',y1+15,' action : ', action, '\n')
fen1.after(1000, show_info)
def physic_sim(action,x, x_dot, theta, theta_dot):
g = 9.81
mass_cart = 1.0
mass_pole = 0.1
total_mass = mass_cart+mass_pole
length_cable = 0.5
pole_mass_length = mass_pole * length_cable
force_magnitude = 10.0
tau = 0.02 # pas d'integration
#global action# depend d'une autre fonction
# if action >0:
# force = force_magnitude
# else:
# force = -force_magnitude
force = action * force_magnitude
temp = (force + pole_mass_length * pow(theta_dot,2) * sin(theta))/total_mass
theta_accel = (g*sin(theta)-cos(theta)*temp)/(length_cable*(4/3-mass_pole*pow(cos(theta),2)/total_mass))
x_accel = temp-pole_mass_length*theta_accel*cos(theta)/total_mass
# --- update_state_variable --- #
x_dot = x_dot + tau*x_accel
x += tau*x_dot
theta_dot += tau*theta_accel
theta += tau*theta_dot
# avance_all_cartesian(x,theta, length_cable)
# fen1.after(50, physic_sim)
return x, x_dot, theta, theta_dot
# gestionnaires d'événements :
def depl_gauche():
avance(-10, 0)
def depl_cart_gauche():
toto=0
#avance_cart(-10,0)
def depl_droite():
avance(10, 0)
def depl_cart_droite():
toto=0
#avance_cart(10,0)
def depl_haut():
avance(0, -10)
def depl_bas():
avance(0, 10)
def clavier(event):
touche = event.keysym
if touche == "Left":
depl_cart_gauche()
elif touche == "Up":
depl_haut()
elif touche == "Right":
depl_cart_droite()
elif touche == "Down":
depl_bas()
#------ Programme principal ------- # les variables suivantes seront utilisées de manière globale :
height = 800
width = 1000
widthrect1, heightrect1 = 80, 50
xrect1, yrect1 = (width/2)-(widthrect1/2), 3*(height/4)-(heightrect1/2)
x1, y1 = xrect1 + widthrect1/2, yrect1 - 30
# x, x_dot, theta, theta_dot = 0,0,0,0
# action = 0
#coordonnées initiales # Création du widget principal ("maître") :
# fen1 = Tk()
# fen1.title("Pendule inversé v0.1")
# # création des widgets "esclaves" :
# can1 = Canvas(fen1,bg='white',height=1000,width=1000)
# can1.focus_set()
# can1.bind("<Key>",clavier)
# oval1 = can1.create_oval(x1,y1,x1+30,y1+30,width=1,fill='red')
# rect1 = can1.create_rectangle(xrect1, yrect1, xrect1+widthrect1, yrect1+heightrect1, fill="blue")
# line1 = can1.create_line(0,3*(height/4)+heightrect1, width, 3*(height/4)+heightrect1,width=2,fill="black")
# line2 = can1.create_line(x1+15,y1+15,xrect1+(widthrect1/2),yrect1,width=3, fill="red")
# can1.pack(side=LEFT)
# Button(fen1,text='Quitter',command=fen1.quit).pack(side=BOTTOM)
# Button(fen1,text='Gauche',command=depl_gauche).pack()
# Button(fen1,text='Droite',command=depl_droite).pack()
# Button(fen1,text='Haut',command=depl_haut).pack()
# Button(fen1,text='Bas',command=depl_bas).pack()
# # # démarrage du réceptionnaire d'évènements (boucle principale) :
# fen1.after(50, physic_sim)
# fen1.after(50, episode)
# fen1.after(1000, show_info)
# fen1.mainloop()