Raspberry Pi och Wiimote kontrollerade Robot Arm (5 / 6 steg)

Steg 5: Koden

jag följde några av de webbplatser som anges ovan för att få en uppfattning av hur koden fungerar, men all kod i mitt program är mitt eget arbete. Jag hoppas jag inte skrämmer någon bort med den. 75% av det är mestadels kommentarer och liten del kvar är själva koden. Min förhoppning är att människor kommer att kunna förstå det och göra det egna. Jag är säker på att jag gjorde några över döda i det också och att det skulle kunna förenklas ganska lite, men det finns många sätt att flå en katt.

Du kan styra armen med eller utan nunchuk. Jag kunde aldrig hitta ett program som används för alla saker i nunchuk (styrspaken, knappar och accelerometern) så jag ville vara säker på att det var ett program som hade allt i det och var lätt att förstå så att människor kunde göra vad de ville med wiimote och nunchuk. För att åstadkomma detta såg jag till att varje knapp eller sensorn eller tillbehör användes så att människor kan anpassa det för sina egna syften. Därför några av koden kan tyckas överflödigt, men det finns en anledning bakom det. Det enda som jag inte använder var talaren (ingen kan få det att fungera ännu) och IR-sensorn.

Känn dig fri att ta denna kod och använda den som du vill!

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

''' Först måste vi importera vissa filer (dessa filer innehåller alla kommandon som behövs för vårt program)
Vi har usb.core och usb.util - dessa används för att kontrollera USB-porten för vår arm
Nästa har vi cwiid som kommunicerar med wiimote
Och vi har tid biblioteket som tillåter oss att sakta eller pausa saker '''
importera usb.core, usb.util, cwiid, tid

#Give vår robot arm ett lätt namn så att vi bara behöver ange allt skräp krävs för USB-anslutningen en gång
skriva ut "Se till arm är redo att gå."
skriva ut ''
ARMC = 1750
Arm = ingen
medan (Arm == ingen):
#This ansluter till usb
Arm = usb.core.find (idVendor = 0x1267, idProduct = 0x0000)
#This kommer att vänta en sekund, och sedan om programmet inte kunde ansluta, det berättar och försöker igen
ARMC = Armc + 1
om (Armc == 2000):
skriva ut "Kunde inte ansluta till Arm, dubbelkolla dess anslutningar."
skriva ut "Programmet kommer att fortsätta när anslutning upprättas..."
skriva ut ""
ARMC = Armc/2000
fortsätta

#Set upp vår arm transfer protocol via usb och definiera ett värde vi kan förändra för att styra armen
Längd = 1
ArmLight = 0

#Create försening variabel som vi kan använda (sekunder)
Delay = 1.
Räknare = 9999
def ArmMove (varaktighet, ArmCmd):

#Start rörelse
Arm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)
Time.Sleep(duration)

#Stop rörelse
ArmCmd = [0,0, ArmLight]
Arm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)

#Establish en anslutning med wiimote
skriva ut "Connected att beväpna framgångsrikt."
skriva ut ""
skriva ut "Tryck på 1 och 2 på wiimote på samma gång."
#Connect mote och om det inte går att ansluta sedan det berättar och försök igen
Time.Sleep(3)
skriva ut ''
skriva ut "upprätta anslutning... 5'
Time.Sleep(1)
skriva ut "upprätta anslutning... 4'
Time.Sleep(1)
skriva ut "upprätta anslutning... 3'
Wii = ingen
medan (Wii == ingen):
försök:
Wii = cwiid. Wiimote()
utom RuntimeError:
skriva ut "Fel vid anslutning till wiimote, tryck 1 och 2."
skriva ut "upprätta anslutning... 2'
Time.Sleep(1)
skriva ut "upprätta anslutning... 1'
Time.Sleep(1)
skriva ut ''

#Once en anslutning har upprättats med de två enheterna resten av programmet kommer att fortsätta; annars, det kommer att fortsätta att försöka att ansluta de två enheterna

#Rumble att ange anslutning och vända på LED
Wii.Rumble = 1 #1 = på, 0 = off
Skriv ut "-anslutning upprättats."
skriva ut "Tryck på valfri knapp för att fortsätta..."
skriva ut ''

''' Varje nummer, aktiveras olika lysdioder på wiimote
ex) om Wii.led = 1, så LED 1 är på
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 1,2,4 12 = LED 3,4 13 = LED 1,3,4
14 = LED 2,3,4 15 = LED 1,2,3,4
Det räknas i binär till 15'' '
Time.Sleep(1)
Wii.Rumble = 0
Wii.LED = 15

# Ställ in den så att vi kan tala om när och vad knappar skjuts, och göra det så att accelerometer indata kan läsas
Wii.rpt_mode = cwiid. RPT_BTN | cwiid. RPT_ACC | cwiid. RPT_EXT
Wii.State

samtidigt sant:

#This behandlar accelerometern
''' skapa en variabel som innehåller x-accelerometer värde
(ändringar om mote vrids eller knäppt vänster eller höger)
platta eller upp och ned = 120, om vände: 90 grader cc = 95, 90 grader c = 145'' '
Accx = (Wii.state['acc'][cwiid. X])

''' skapa en variabel som innehåller värdet y accelerometer
(ändringar när mote är pekade eller knäppt upp eller ner)
platt = 120, IR pekar uppåt = 95, IR pekar nedåt = 145'' '
Accy = (Wii.state['acc'][cwiid. Y])

''' skapa en variabel som innehåller z accelerometer värde
(Ändringar med motes rotation, eller när den dras tillbaka eller knäppt upp/ned)
platt = 145, 90 grader cc eller c, eller 90 grader upp och ner = 120, upp och ned = 95'' '
Accz = (Wii.state['acc'][cwiid. Z])

#This erbjudanden med knappar, berättar vi varje knapp vad vi vill göra
knappar = Wii.state['buttons']
#Get batteritid (i procent av 100):
#Just ta bort nunber logga inn framsidan
#print Wii.state['battery']*100/cwiid. BATTERY_MAX

# Om hem-knappen trycks sedan rumble och sluta, plus Stäng program
om (knappar & cwiid. BTN_HOME):
skriva ut ''
skriva ut "Stänga anslutning..."
ArmLight = 0
ArmMove(.1,[0,0,0])
Wii.Rumble = 1
Time.Sleep(.5)
Wii.Rumble = 0
Wii.LED = 0
Exit(Wii)

''' Arm kommandon definieras av ArmMove är
[0,1,0] rotera bas medurs
[0,2,0] rotera basen C-medurs
[64,0,0] skuldra upp
[128,0,0] knuffa ner
[16,0,0] armbåge upp
[32,0,0] armbåge ned
[4,0,0] handled upp
[8,0,0] handleden ner
[2,0,0] grepp öppen
[1,0,0] grepp i närheten
[0,0,1] ljus på
[0,0,0] ljus av

ex) ArmMove (varaktighet i seconds,[0,0,0])
Detta exempel skulle stoppa all rörelse och stänga av lampan '''

#Check att se om andra knappar trycks
om (knappar & cwiid. BTN_A):
skriva ut "En pressad"
Time.Sleep(delay)
ArmMove(.1,[1,0,ArmLight])
om (knappar & cwiid. BTN_B):
skriva ut "B tryckte"
Time.Sleep(delay)
ArmMove(.1,[2,0,ArmLight])
om (knappar & cwiid. BTN_1):
Skriv ut '1 tryckte'
ArmMove(.1,[16,0,ArmLight])
om (knappar & cwiid. BTN_2):
Print "2 pressade"
ArmMove(.1,[32,0,ArmLight])
om (knappar & cwiid. BTN_MINUS):
skriva ut "Minus pressade"
ArmMove(.1,[8,0,ArmLight])
om (knappar & cwiid. BTN_PLUS):
skriva ut "Plus pressade"
ArmMove(.1,[4,0,ArmLight])
om (knappar & cwiid. BTN_UP):
skriva ut "upp pressade"
ArmMove(.1,[64,0,ArmLight])
om (knappar & cwiid. BTN_DOWN):
skriva ut "ner pressad"
ArmMove(.1,[128,0,ArmLight])
om (knappar & cwiid. BTN_LEFT):
skriva ut "Vänster intryckt"
ArmMove(.1,[0,2,ArmLight])
om (knappar & cwiid. BTN_RIGHT):
skriva ut "Rätt tryckte"
ArmMove(.1,[0,1,ArmLight])

#Here vi hanterar nunchuk, tillsammans med joystick och knappar
While(1):
om Wii.state.has_key('nunchuk'):
försök:
#Here är data för nunchuk stick:
#X axis:LeftMax = 25, mitten = 125, RightMax = 225
NunchukStickX = (Wii.state['nunchuk']['stick'][cwiid. X])
#Y-axeln: DownMax = 30, mitten = 125, UpMax = 225
NunchukStickY = (Wii.state['nunchuk']['stick'][cwiid. Y])
#The 'NunchukStickX' och 'NunchukStickY' variablerna nu lagra stick värden

#Here tar vi hand om alla våra data för accelerometern

#The nunchuk har en accelerometer som spelar på ett liknande sätt till wiimote, men antalet är olika
#The X är: 70 om svängas 90 grader till vänster och 175 om lutas 90 grader åt höger
NAccx = Wii.state['nunchuk']['acc'][cwiid. X]
#The Y är: 70 om lutas 90 grader ner (knapparna pekar nedåt) och 175 om lutas 90 grader uppåt (knappar pekar uppåt)
NAccy = Wii.state['nunchuk']['acc'][cwiid. Y]
#I fortfarande förstår inte z-axeln helt (på wiimote och nunchuk), men såvitt jag kan säga är det största förändringen kommer från direkt dra upp grandet utan luta den
NAccz = Wii.state['nunchuk']['acc'][cwiid. Z]

#Make det så att vi kan kontrollera armen med joystick
om (NunchukStickX < 60):
ArmMove(.1,[0,2,ArmLight])
skriva ut "Flytta vänster"
om (NunchukStickX > 190):
ArmMove(.1,[0,1,ArmLight])
skriva ut "Flytta rätt"
om (NunchukStickY < 60):
ArmMove(.1,[128,0,ArmLight])
skriva ut "Flytta ner"
om (NunchukStickY > 190):
ArmMove(.1,[64,0,ArmLight])
skriva ut "Flytta upp"

#Make det så att vi kan kontrollera armen med tilt funktioner
#Left till höger
om (Accx < 100 och NAccx < 90):
ArmMove(.1,[0,2,ArmLight])
skriva ut "Flytta vänster"
om (Accx > 135 och NAccx > 150):
ArmMove(.1,[0,1,ArmLight])
skriva ut "Flytta rätt"

#Up och ner
om (Accy < 100 och NAccy < 90):
ArmMove(.1,[64,0,0])
skriva ut "Flytta upp"
om (Accy > 135 och NAccy > 150):
ArmMove(.1,[128,0,0])
skriva ut "Flytta ner"

#Here skapar vi en variabel för att lagra nunchuck knappen data
#0 = inga knappar tryckte
#1 = Z trycks
#2 = C trycks
#3 = både C och Z trycks

ChukBtn = Wii.state['nunchuk']['buttons']
om (ChukBtn == 1):
skriva ut "Z tryckte"
ArmLight = 0
ArmMove(.1,[0,0,ArmLight])
om (ChukBtn == 2):
skriva ut "C tryckte"
ArmLight = 1
ArmMove(.1,[0,0,ArmLight])
#If båda är tryckte de led blinkar
om (ChukBtn == 3):
skriva ut "C och Z tryckte"
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 andra åtgärder som kräver användning av nunchuk på något sätt måste sättas här för felhantering ska fungera
Break

#This del ner nedan är den del som berättar om ingen nunchuk är ansluten till wiimote
utom KeyError:
skriva ut "Ingen nunchuk upptäckt."
annat:
om (ArmLight == 0):
om (Accz > 179 eller Accz < 50):
ArmLight = 1
ArmMove(.1,[0,0,ArmLight])
Time.Sleep(.5)
Elif (ArmLight == 1):
om (Accz > 179 eller Accz < 50):
ArmLight = 0
ArmMove(.1,[0,0,ArmLight])
Time.Sleep(.5)

om (Counter == 10000):
skriva ut "Ingen nunchuk upptäckt."
Räknare = Counter/10000
Break
Räknare = räknare + 1
Break

Kopiera koden i en python-editor och spara ut. Om redaktören är öppen kan du köra programmet med F5. Du kan också köra det genom att öppna en terminal, navigera till platsen för programmet och sedan skriva: sudo python filename.py
Bara ersätta filnamn med det faktiska namnet på filen (du har fortfarande att kopiera koden till redigeraren och sedan spara den).
Bilden visar: sudo Nunchuk_W_Arm.py, men jag glömde att tillägga python därinne. Det ska vara: sudo python Nunchuk_W_Arm.py
Jag jobbar nu på en version av programmet som har en faktisk gränssnitt med knappar att styra arm och grejer som visar den wiimote godsaker. Wiimote kan fortfarande användas för att styra armen; Det är bara en mer visuell program.