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]") |
Partager