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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
|
import Adafruit_PCA9685
import time
import threading #je ne sais pas encore si je vais m'en servir et comment ?
import sys
import RPi.GPIO as GPIO
import socket
def menuDemandeMouve(): #petit menu en shell pour tester le programme
print ("1.Mouvement complet")
print ("2.Mouvement détaillé d'une jambe")
print ("3.Mouvement détaillé d'un servo")
commandeA = input("Quel type de mouvement ?")
if commandeA == "1" :
print ("z.Avance")
print ("s.Recul")
print ("q.Pas Lateral Gauche")
print ("d.Pas Lateral Droit")
print ("a.Tourne sur la Gauche")
print ("e.Tourne sur la Droite")
print ("w.Pattes Haute")
print ("x.Pattes Basse")
print ("c.Donne la patte")
commandeB = input("Quel mouvement ?")
if self.mouveDemande == 1:
zAvance()
elif self.mouveDemande == 2:
sRecul()
elif self.mouveDemande == 3:
qPasLateralGauche()
elif self.mouveDemande == 4:
dPasLateralDroit()
elif self.mouveDemande == 5:
aTourneGauche()
elif self.mouveDemande == 6:
eTourneDroite()
elif self.mouveDemande == 7:
wPatteHaute()
elif self.mouveDemande == 8:
xPatteBasse()
elif self.mouveDemande == 9:
cLevePatte()
os.system('clear')
menuDemandeMouve()
elif commandeA == "2":
print ("1.Jambe I")
print ("2.Jambe II")
print ("3.Jambe III")
print ("4.Jambe IV")
self.numJambe = int(input("Quel jambe choisi ?"))
#rotation de la jambe avec affichage des infos
os.system('clear')
menuDemandeMouve()
elif commandeA == "3":
numServo = int(input ("Port du servo ? (de 0 à 11)"))
servoPositObjectif = int(input ("Choix du pas ? (de 100 à 560)"))
print (servoCtrl.servoPositActu[numServo])
vitesseServo = vitesseServo
mouveServo(numServo,servoPositActu, servoPositObjectif, vitesseServo)
servoPositActu[numServo] = servoPositObjectif
#pwm.set_pwm(numPort,0,numPas)
os.system('clear')
menuDemandeMouve()
class servoCtrl():
#initialisation des servos
def initAllServo():
pwm = Adafruit_PCA9685.PCA9685()
pwm.set_pwm_freq(50) #frequenceServo
nombreServo = 11 #en comptant à partir de 0
servoPort = [0,1,2,3,4,5,6,7,8,9,10,11]
servoDirection = [1,-1,-1,-1,1,1,-1,1,1,1,-1,-1]
servoPositIni = [312,305,312,312,287,260,325,281,301,305,195,340]
servoPositActu = [312,305,312,312,287,260,325,281,301,305,195,340]
servoMinRange = [100,100,100,100,100,100,100,100,100,100,100,100]
servoMaxRange = [560,560,560,560,560,560,560,560,560,560,560,560]
vitesseServo = 100
for i in range(0, nombreServo):
pwm.set_pwm(i, 0, servoPositIni[i]) #(numéro du port, déviation mais non utilisé avec ces servos, position voulu)
#cette méthode permet de définir/identifier les 3 servos de chacunes des jambes
def servoJambe(self, numJambe):
j=(numJambe-1)*3
self.servoA = self.servoPort[j] #servoA pour gérer l'avant/arrière des pattes
self.servoB = self.servoPort[j+1] #servoB pour gérer la hauteur des pattes
self.servoC = self.servoPort[j+2] #servoC pour gérer l'écartement des pattes
return self.servoA, self.servoB, self.servoC ;
#cette méthode permet de calculer le pas attendu sur un servo en fonction de son sens de rotation/montage sur la jambe
def directionServo(self, numServo, servoPositObjectif): #numServo = n°du servo = port de connexion
if servoDirection [numServo] == -1 :
self.servoPositObjectif = 560-(servoPositObjectif-100)
else:
self.servoPositObjectif = self.servoPositObjectif
return servoPositObjectif
#cette méthode "élémentaire" créer la rotation d'un servo en fonction : de son port, de sa position actuelle, la position à atteindre et à une vitesse demander par défaut
#qui est instancié autant de fois qu'il y a de servo à mettre en mouvement
def mouveServo(self, numServo, servoPositActu, servoPositObjectif, vitesseServo):
for k in range (servoPositActu, servoPositObjectif,vitesseServo):
pwm.set_pwm(numServo, 0, 2) #le servo tourne par pas de 2 mais puis fait une pause de 1/100 de seconde
time.sleep(1/vitesseServo)
self.servoPositActu = self.servoPositObjectif
return servoPositActu
#méthode principale qui appel les autres
#qui est à instancier 4 fois lors d'un déplacement pour les 4 jambes sauf cas exceptionnel (cas donne une patte)
def mouveJambe (self, numJambe):
servoJambe(self,numJambe) #donne les numéros des servos
self.numServo = self.servoA #numéro du servo à faire bouger
self.servoPositActu = self.servoPositActu #position du servo issu du dernier mouvement ou de l'initialisation
#self.servoPositObjectif #donne la position à obtenir pour un servo, doit être donnée par l'instance maitre
#self.mouveServoA[i] #qu'est ce que i et quel et sa valeur ?
self.directionServo(self.numServo, self.servPositObjectif) #vérifie la position à obtenir en fonction du sens de montage du servo
self.vitesseServo = self.vitesseServo #définit la vitesse de déplacement
mouveServo(self.numServo, self.servoPositActu, self.servoPositObjectif, self.vitesseServo) #lance le mouvement
self.numServo = self.servoB #numéro du servo à faire bouger
self.servoPositActu = self.servoPositActu #position du servo issu du dernier mouvement ou de l'initialisation
#self.servoPositObjectif #donne la position à obtenir pour un servo, doit être donnée par l'instance maitre
#self.mouveServoA[i] #qu'est ce que i et quel et sa valeur ?
self.directionServo(self.numServo, self.servPositObjectif) #vérifie la position à obtenir en fonction du sens de montage du servo
self.vitesseServo = self.vitesseServo #définit la vitesse de déplacement
mouveServo(self.numServo, self.servoPositActu, self.servoPositObjectif, self.vitesseServo) #lance le mouvement
self.numServo = self.servoC #numéro du servo à faire bouger
self.servoPositActu = self.servoPositActu #position du servo issu du dernier mouvement ou de l'initialisation
#self.servoPositObjectif #donne la position à obtenir pour un servo, doit être donnée par l'instance maitre
#self.mouveServoA[i] #qu'est ce que i et quel et sa valeur ?
self.directionServo(self.numServo, self.servPositObjectif) #vérifie la position à obtenir en fonction du sens de montage du servo
self.vitesseServo = self.vitesseServo #définit la vitesse de déplacement
mouveServo(self.numServo, self.servoPositActu, self.servoPositObjectif, self.vitesseServo) #lance le mouvement
def positObjectif (self): #Etat de jambe
#pas des servos
positABC1=300,560,300 #1 = jambe à mi chemin, en position haute,
positABC2=560,300,300 #2 = jambe à mi hauteur, posé au sol sur l'avant
positABC3=300,300,300 #3 = jambe à mi hauteur, posé au sol sur le milieu/à l'axe de la jambe
positABC4=100,300,300 #4 = jambe à mi hauteur, posé au sol à l'arrière
positABC5=300,100,300 #5 = jambe basse, posé au sol sur le milieu/à l'axe de la jambe
positABC6=300,560,300 #6 = jambe haute, posé au sol sur le milieu/à l'axe de la jambe
positABC7=560,560,100 #7 = jambe haute en l'aire, sur l'avant
positABC8=300,300,400 #8 = jambe proche
positABC9=300,300,100 #9 = jambe éloigné
return positABC1,positABC2,positABC3,positABC4,positABC5,positABC6,positABC7,positABC8,positABC9
# def zAvance()
# #va chercher les valeurs objectives
# positABC1,positABC2,positABC3,positABC4 = positObjectif(positABC1,positABC2,positABC3,positABC4)
# #lance le mouvement sur chaque jambe
# mouveJambe(self,1,positABC1)
# mouveJambe(self,2,positABC2)
# mouveJambe(self,3,positABC3)
# mouveJambe(self,4,positABC4)
# mouveJambe1 = 1,2,3,4
# mouveJambe2 = 2,3,4,1
# mouveJambe3 = 3,4,1,2
# mouveJambe4 = 4,1,2,3
if __name__ == "__main__":
servoCtrl.initAllServo()
while True:
menuDemandeMouve()
else:
print ("Initialisation subServoMouve.py"+"\t\t\t[Ok]") |
