-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathtemplate.py
More file actions
135 lines (108 loc) · 3.65 KB
/
Copy pathtemplate.py
File metadata and controls
135 lines (108 loc) · 3.65 KB
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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
# Bibliothèques nécessaires pour que le code fonctionne
from hub import light_matrix, port, motion_sensor
import runloop, motor, color, color_sensor, math, time, motor_pair
# Modifier les ports selon vos branchements ici
port_moteur_roue_droite = port.D
port_moteur_roue_gauche = port.C
port_moteur_module_gauche = port.A
port_moteur_module_droit = port.B
port_capteur_couleur_droit = port.F
port_capteur_couleur_gauche = port.E
# Informations sur le robot
circonference_roue = 17.5
roues = 0
motor_pair.pair(roues, port_moteur_roue_gauche, port_moteur_roue_droite)
# Fonction permettant au robot d'avancer d'une certaine distance en ligne droite à l'aide du gyroscope et d'un contrôleur PID
def bouger_CM(distance):
motor.reset_relative_position(port_moteur_roue_droite, 0)
motor.reset_relative_position(port_moteur_roue_gauche, 0)
time.sleep_ms(250)
motion_sensor.reset_yaw(0)
time.sleep_ms(250)
gyro = motion_sensor.tilt_angles()
PV = 0
# Définition du setpoint en degrées
SP = distance / 17.5 * 360
t = 0
# Initialisation des paramètres du PID
eD_prev = 0
t_prev = -100
I = 0
Kp = 3.0
Ki = 0.1
Kd = 0.5
beta = 1
gamma = 0
I_max = 100# Limite pour l'intégrale
while(abs(SP-PV) > 3):
t += 1
# Calcule de la distance en degrées parcouru par la roue depuis le début
PV = (motor.relative_position(port_moteur_roue_droite) - motor.relative_position(port_moteur_roue_gauche)) / 2
# Calcul du PID
P = Kp*(beta*SP - PV)
I = I + Ki*(SP - PV)*(t - t_prev)
I = max(min(I, I_max), -I_max)# Limite l'intégrale
eD = gamma*SP - PV
D = Kd*(eD - eD_prev)/(t - t_prev)
MV = P + I + D
# Mise à jour des paramètres du PID
eD_prev = eD
t_prev = t
#Ajoute la correction du gyro pour assurer une ligne droite
motor_pair.move_tank(roues, int(MV + gyro[0]), int(MV - gyro[0]))
# Arrete les roues une fois rendu a destination
motor_pair.stop(roues)
# Fonction permettant au robot de tourner selon un angle précis à l'aide du gyroscope et d'un contrôleur PID
def tourner_gyro(angle):
time.sleep_ms(250)
motion_sensor.reset_yaw(0)
time.sleep_ms(250)
gyro = motion_sensor.tilt_angles()
PV = gyro[0]
# Définition du setpoint
SP = -angle * 10
t = 0
eD_prev = 0
t_prev = -100
I = 0
Kp = 0.35
Ki = 0.00005
Kd = 0.15
beta = 1
gamma = 0
I_max = 50 # Limite pour l'intégrale
dead_zone = 25 # Zone morte pour éviter les petites oscillations
while(abs(SP - PV) > dead_zone):
t += 1
gyro = motion_sensor.tilt_angles()
PV = gyro[0]
print(PV)
# Calcul du PID
P = Kp*(beta*SP - PV)
I = I + Ki*(SP - PV)*(t - t_prev)
I = max(min(I, I_max), -I_max)# Limite l'intégrale
eD = gamma*SP - PV
D = Kd*(eD - eD_prev)/(t - t_prev)
MV = P + I + D
# Mise à jour des paramètres du PID
eD_prev = eD
t_prev = t
motor_pair.move_tank(roues, -int(MV), int(MV))
# Arrête les roues une fois rendu a destination
motor_pair.stop(roues)
async def main():
# Écrire votre code ici
bouger_CM(-2)
bouger_CM(30)
tourner_gyro(-50)
bouger_CM(20)
bouger_CM(-20)
tourner_gyro(50)
motor.run_for_degrees(port_moteur_module_gauche, -90, 1110)
time.sleep_ms(500)
motor.run_for_degrees(port_moteur_module_gauche, 90, 1110)
time.sleep_ms(500)
motor.run_for_degrees(port_moteur_module_droit, -90, 1110)
time.sleep_ms(500)
motor.run_for_degrees(port_moteur_module_droit, 90, 1110)
runloop.run(main())