Raspberry Pi et Wiimote contrôlée Robot Arm (5 / 6 étapes)

Étape 5: Le Code

j’ai suivi certains des sites Web énumérés ci-dessus pour avoir une idée de comment le code fonctionne, mais tout le code dans mon programme est mon propre travail. J’espère que je ne faire peur à quelqu'un avec elle. 75 % de celui-ci est pour la plupart des commentaires et la petite partie restante est le code réel. Mon espoir est que les gens seront en mesure de comprendre et de faire leurs propres. Je suis sûr que j’ai fait quelques sur tuer dedans trop et qu’il pourrait être un peu simplifié, mais il y a bien des égards à la peau d’un chat.

Vous pouvez contrôler le bras avec ou sans le nunchuk. Je ne pouvais jamais trouver un programme qui utilise tous les trucs dans le nunchuk (le manche à balai, les boutons et l’accéléromètre) alors je voulais faire ce qu’il y avait un programme qui avait tout dedans et a été assez facile à comprendre pour que les gens pouvait faire ce que jamais ils voulaient avec le nunchuk wiimote de et . Pour ce faire, je me suis assuré que chaque bouton, capteur ou accessoire a été utilisé afin que les gens peuvent personnaliser à leurs propres fins. Pour cette raison, certains du code peuvent paraître redondant, mais il y a une raison derrière elle. Les seules choses que je n’ai pas utilisé étaient l’orateur (personne ne peut faire fonctionner encore) et le capteur IR.

N’hésitez pas à prendre ce code et l’utiliser comme vous le souhaitez !

#   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
#   | T| POINTS A| Y| L| O| R| | B| O| POINTS A| R| D | M| POINTS A| N| | | | R| P| I| | POINTS A| R| M|
#   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+

''' Tout d’abord, nous avons besoin d’importer certains fichiers (ces fichiers contiennent toutes les commandes nécessaires à notre programme)
Nous avons usb.core et usb.util - ils sont utilisés pour contrôler le port usb pour nos bras
Ensuite, nous avons cwiid qui communique avec la wiimote
Et nous avons la bibliothèque qui permet de ralentir ou d’arrêter les choses '''
importer des usb.core, usb.util, cwiid fois

#Give notre robot bras un nom simple pour que nous avons seulement besoin de spécifier tous les indésirables requis pour la connexion usb une fois
imprimer « S’assurer que le bras est prêt à aller. »
d’impression ''
ARMC = 1750
Bras = None
tandis que (bras == None) :
#This se connecte au port usb
Bras = usb.core.find (idVendor = 0x1267, idProduct = 0 x 0000)
#This attendra pendant une seconde, et puis si le programme ne pouvait pas se connecter, il nous dit et essaie à nouveau
ARMC = Armc + 1
Si (Armc == 2000) :
imprimer « Could not connect to Arm, vérifiez ses connexions. »
imprimer « Le programme se poursuivra lorsque la connexion est établie... »
imprimer ' '
ARMC = Armc/2000
continuer

#Set nos bras transfer protocole par l’intermédiaire de l’usb et définir une valeur pour contrôler le bras, nous pouvons changer
Durée = 1
Rachid = 0

#Create variable de retard que nous pouvons utiliser (secondes)
Retard = 1.
Compteur = 9999
def ArmMove (durée, ArmCmd) :

#Start mouvement
ARM.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)
Time.Sleep(Duration)

#Stop mouvement
ArmCmd = [0,0, Rachid]
ARM.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)

#Establish une connexion avec la wiimote
imprimer « Connected pour armer avec succès. »
imprimer ' '
imprimer « Appuyez sur 1 et 2 sur la wiimote en même temps. »
#Connect de paille et si elle n’est pas connecter puis il nous dit et essaie à nouveau
Time.Sleep(3)
d’impression ''
imprimer "Establishing Connection... 5'
Time.Sleep(1)
imprimer "Establishing Connection... 4'
Time.Sleep(1)
imprimer "Establishing Connection... 3'
Wii = None
tandis que (Wii == None) :
Essayez :
Wii = cwiid. Wiimote()
sauf RuntimeError :
Print « Erreur de connexion à la wiimote, appuyez sur 1 et 2. »
imprimer "Establishing Connection... 2'
Time.Sleep(1)
imprimer "Establishing Connection... 1'
Time.Sleep(1)
d’impression ''

#Once une connexion a été établie avec les deux appareils, que le reste du programme se poursuivra ; dans le cas contraire, il va continuer à essayer de se connecter à deux dispositifs

#Rumble pour indiquer la connexion et la LED s’allume
Wii.Rumble = 1 #1 = activé, 0 = off
imprimer « Connexion établie ».
imprimer « Appuyez sur n’importe quelle touche pour continuer... »
d’impression ''

''' Chaque nombre tours sur différents voyants lumineux de la wiimote
ex) if Wii.led = 1, 1 LED est sur
2 = LED 2 3 = LED 3 4 = LED 4
5 = LED 1, 3 6 = LED 2, 3 7 = LED 1,2,3
8 = LED 4 9 = LED 1, 4 10 = LED 2,4
11 = LED 12 1,2,4 = LED 13 3,4 = LED 1,3,4
14 = LED 15 2,3,4 = LED 1,2,3,4
Il compte en binaire à 15'' '
Time.Sleep(1)
Wii.Rumble = 0
Wii.LED = 15

# Réglez-le de sorte que nous puissions dire quand et quels boutons sont poussés et faire en sorte que l’entrée d’accéléromètre peut être lu
Wii.rpt_mode = cwiid. RPT_BTN | Cwiid. RPT_ACC | Cwiid. RPT_EXT
Wii.State

While True :

#This traite de l’accéléromètre
''' créer une variable contenant la valeur de x accéléromètre
(changements si mote est tourné ou dévié à gauche ou à droite)
plat ou à l’envers = 120, si activé : 90 degrés cc = 95, 90 degrés c = 145'' '
ACCx = (Wii.state['acc'][cwiid. X])

''' créer une variable contenant la valeur y de l’accéléromètre
(changements quand mote est pointé ou dévié vers le haut ou vers le bas)
plat = 120, IR vers le haut = 95, IR vers le bas = 145'' '
ACCY = (Wii.state['acc'][cwiid. Y])

''' créer une variable contenant la valeur de z accéléromètre
(Changements avec la rotation de motes, ou quand tiré arrière ou châtain haut/bas)
plat = 145, 90 degrés c cc ou 90 degrés verticalement = 120, upside-down = 95'' '
AccZ = (Wii.state['acc'][cwiid. Z])

#This traite les boutons, nous disons chaque bouton ce que nous voulons qu’il fasse
boutons = 'Wii.state['buttons]
#Get vie de la batterie (en proportion de 100) :
#Just supprimer le front inn de nombre signe
#print Wii.state['battery']*100/cwiid. BATTERY_MAX

# Si vous appuyez sur le bouton home puis rumble et cesser de fumer, et fermer le programme
Si (boutons & cwiid. BTN_HOME) :
d’impression ''
"Fermeture connexion d’impression..."
Rachid = 0
ArmMove(.1,[0,0,0])
Wii.Rumble = 1
Time.Sleep(.5)
Wii.Rumble = 0
Wii.LED = 0
Exit(Wii)

''' Bras commandes définies par ArmMove sont
[0,1,0] Base de rotation dans le sens horaire
[0,2,0] rotation Base C-dans le sens horaire
[64,0,0] épaule vers le haut
[128,0,0] épaule vers le bas
[16,0,0] coude vers le haut
[32,0,0] coude vers le bas
[4,0,0] poignet vers le haut
[8,0,0] poignet vers le bas
Open de grip [2,0,0]
[1,0,0] grip fermer
[0,0,1] lumière sur
[0,0,0] lumière éteinte

ex) ArmMove (durée en seconds,[0,0,0])
Cet exemple devrait cesser tout mouvement et éteindre la LED'' '

#Check pour voir si les autres boutons sont pressés
Si (boutons & cwiid. BTN_A) :
imprimer « Pressé »
Time.Sleep(Delay)
ArmMove(.1,[1,0,ArmLight])
Si (boutons & cwiid. BTN_B) :
imprimer "B enfoncé"
Time.Sleep(Delay)
ArmMove(.1,[2,0,ArmLight])
Si (boutons & cwiid. BTN_1) :
Print « 1 appuie sur »
ArmMove(.1,[16,0,ArmLight])
Si (boutons & cwiid. BTN_2) :
Print « 2 appuie sur »
ArmMove(.1,[32,0,ArmLight])
Si (boutons & cwiid. BTN_MINUS) :
imprimer "moins pressé"
ArmMove(.1,[8,0,ArmLight])
Si (boutons & cwiid. BTN_PLUS) :
imprimer « Plus pressé »
ArmMove(.1,[4,0,ArmLight])
Si (boutons & cwiid. BTN_UP) :
imprimer 'Up pressé"
ArmMove(.1,[64,0,ArmLight])
Si (boutons & cwiid. BTN_DOWN) :
imprimer 'Down pressé"
ArmMove(.1,[128,0,ArmLight])
Si (boutons & cwiid. BTN_LEFT) :
imprimer « Gauche enfoncé »
ArmMove(.1,[0,2,ArmLight])
Si (boutons & cwiid. BTN_RIGHT) :
imprimer « Droite enfoncée »
ArmMove(.1,[0,1,ArmLight])

Nous prenons en charge le nunchuk, avec le joystick et les boutons de #Here
while(1) :
Si Wii.state.has_key('nunchuk') :
Essayez :
#Here est les données pour le stick du nunchuk :
#X axis:LeftMax = 25, middle = 125, RightMax = 225
NunchukStickX = (Wii.state['nunchuk']['stick'][cwiid. X])
#Y axe : DownMax = 30, Middle = 125, UpMax = 225
NunchukStickY = (Wii.state['nunchuk']['stick'][cwiid. Y])
#The « NunchukStickX » et les « NunchukStickY » variables désormais stockent les valeurs de bâton

#Here que nous nous occupons de toutes nos données de l’accéléromètre

#The nunchuk a un accéléromètre qui enregistre de façon similaire à la wiimote, mais la plage de numéros est différente
Gamme de #The X est : if 70 incliné de 90 degrés vers la gauche et 175 s’incliné de 90 degrés vers la droite
NAccx = Wii.state['nunchuk']['acc'][cwiid. X]
Gamme de #The Y est : if 70 incliné de 90 degrés vers le bas (les boutons vers le bas) et 175 s’incliné de 90 degrés vers le haut (vers le haut des boutons)
Clara = Wii.state['nunchuk']['acc'][cwiid. Y]
#I encore ne comprennent pas l’axe z complètement (sur la wiimote et le nunchuk), mais que je peux dire c’est le principal changement vient directement tirant vers le haut de la paille sans l’incliner
Nadjadi = Wii.state['nunchuk']['acc'][cwiid. Z]

#Make il sorte que nous pouvons contrôler le bras avec le joystick
Si (NunchukStickX < 60) :
ArmMove(.1,[0,2,ArmLight])
imprimer « Se déplaçant à gauche »
Si (NunchukStickX > 190) :
ArmMove(.1,[0,1,ArmLight])
imprimer « Droit de déplacement »
Si (NunchukStickY < 60) :
ArmMove(.1,[128,0,ArmLight])
imprimer « Déplacer vers le bas »
Si (NunchukStickY > 190) :
ArmMove(.1,[64,0,ArmLight])
imprimer 'Moving Up'

#Make il sorte que nous pouvons contrôler le bras avec inclinaison des fonctions
#Left à droite
Si (Accx < 100 et NAccx < 90) :
ArmMove(.1,[0,2,ArmLight])
imprimer « Se déplaçant à gauche »
Si (Accx > 135 et NAccx > 150) :
ArmMove(.1,[0,1,ArmLight])
imprimer « Droit de déplacement »

#Up et vers le bas
Si (Accy < 100 et bataille < 90) :
ArmMove(.1,[64,0,0])
imprimer 'Moving Up'
Si (Accy > 135 et bataille > 150) :
ArmMove(.1,[128,0,0])
imprimer « Déplacer vers le bas »

#Here nous créons une variable pour stocker le nunchuck bouton données
#0 ne = aucune pression sur les boutons
#1 = Z est pressé
#2 = C est enfoncée
#3 = les deux C et Z sont pressées

ChukBtn = 'Wii.state['nunchuk']['buttons]
Si (ChukBtn == 1) :
imprimer "Z enfoncée"
Rachid = 0
ArmMove(.1,[0,0,ArmLight])
Si (ChukBtn == 2) :
imprimer "C enfoncée"
Rachid = 1
ArmMove(.1,[0,0,ArmLight])
#If que sont tous deux appuyé la led clignote
Si (ChukBtn == 3) :
imprimer « appuie sur C et Z »
ArmMove(.1,[0,0,0])
Time.Sleep(.25)
ArmMove(.1,[0,0,1])
Time.Sleep(.25)
ArmMove(.1,[0,0,0])
Time.Sleep(.25)
ArmMove(.1,[0,0,1])
Time.Sleep(.25)
ArmMove(.1,[0,0,0])
Time.Sleep(.25)
ArmMove(.1,[0,0,1])
Time.Sleep(.25)
ArmMove(.1,[0,0,0])

#Any autres actions qui nécessitent l’utilisation du nunchuk en quelque sorte doivent être placées ici pour la gestion des erreurs fonctionne correctement
rupture

#This partie vers le bas ci-dessous est la partie qui nous dit si aucune nunchuk n’est reliée à la wiimote
sauf KeyError :
imprimer « Sans nunchuk détecté. »
autre chose :
Si (qocis == 0) :
Si (Accz > 179 ou Accz < 50) :
Rachid = 1
ArmMove(.1,[0,0,ArmLight])
Time.Sleep(.5)
Elif (qocis == 1) :
Si (Accz > 179 ou Accz < 50) :
Rachid = 0
ArmMove(.1,[0,0,ArmLight])
Time.Sleep(.5)

Si (compteur == 10000) :
imprimer « Sans nunchuk détecté. »
Compteur = compteur/10000
rupture
Compteur = compteur + 1
rupture

Copiez le code dans un éditeur de python et puis enregistrez-le dehors. Si l’éditeur est toujours ouvert alors que vous pouvez exécuter le programme avec F5. Vous pouvez également l’exécuter en ouverture d’un terminal, en accédant à l’emplacement du programme et en tapant : sudo python filename.py
Il suffit de remplacer nom de fichier par le nom réel du fichier (vous avez encore à copier le code dans l’éditeur et puis l’enregistrer).
La photo spectacles : sudo Nunchuk_W_Arm.py, mais j’ai oublié d’ajouter le python là-dedans. C’est censé pour être : sudo python Nunchuk_W_Arm.py
Maintenant, je travaille sur une version du programme, qui dispose d’une réelle interface avec boutons pour contrôler les bras et les trucs qui affichent les goodies de la wiimote. La wiimote peut encore être utilisée pour contrôler le bras ; C’est juste un programme plus visuel.