291 lines
5.0 KiB
Python
Executable File
291 lines
5.0 KiB
Python
Executable File
#!/usr/bin/python2.7
|
|
# -*- coding: latin-1 -*-
|
|
|
|
from KranClass import Kran
|
|
from evdev import *
|
|
from select import select
|
|
from multiprocessing import Process
|
|
import time
|
|
#
|
|
#
|
|
# gpio: minus plus greifer handgelenk ellenbogen armgelenk basis licht
|
|
#
|
|
# gpio: 3 5 7 8 11 10 12 16
|
|
# gpio: 40 38 37 36 35 33 32 31
|
|
#
|
|
# key r1: o l p ö ü ä + # . - ,
|
|
# key r2: q a w s e d r f x c y
|
|
#
|
|
# rel 1: in1 in2 in4 in3 in5 in7 in6 in8
|
|
# rel 2: in1 in2 in4 in3 in5 in7 in6 in8
|
|
|
|
robot1 = Kran( 3, 5, 7, 8, 11, 10, 12, 16)
|
|
robot2 = Kran(40, 38, 37, 36, 35, 33, 32, 31)
|
|
|
|
dev = InputDevice('/dev/input/event1')
|
|
|
|
def robot1func(event1):
|
|
|
|
# handgelenk on
|
|
# taste p
|
|
|
|
if event1.code == 25:
|
|
robot1.move("H", 1)
|
|
return 1
|
|
|
|
# handgelenk off
|
|
# taste ö
|
|
|
|
elif event1.code == 39:
|
|
robot1.move("H", 2)
|
|
return 1
|
|
|
|
# ellenbogen on
|
|
# taste ü
|
|
|
|
elif event1.code == 26:
|
|
robot1.move("E", 1)
|
|
return 1
|
|
|
|
# ellenbogen off
|
|
# taste ä
|
|
|
|
elif event1.code == 40:
|
|
robot1.move("E", 2)
|
|
return 1
|
|
|
|
# armgelenk on
|
|
# taste +
|
|
|
|
elif event1.code == 27:
|
|
robot1.move("A", 1)
|
|
return 1
|
|
|
|
# armgelenk off
|
|
# taste #
|
|
|
|
elif event1.code == 43:
|
|
robot1.move("A", 2)
|
|
return 1
|
|
|
|
# basis off
|
|
# taste .
|
|
|
|
elif event1.code == 52:
|
|
robot1.move("B", 2)
|
|
return 1
|
|
|
|
# basis on
|
|
# taste -
|
|
|
|
elif event1.code == 53:
|
|
robot1.move("B", 1)
|
|
return 1
|
|
|
|
# greifer off
|
|
# taste o
|
|
|
|
elif event1.code == 24:
|
|
robot1.move("G", 2)
|
|
return 1
|
|
|
|
# greifer on
|
|
# taste l
|
|
|
|
elif event1.code == 38:
|
|
robot1.move("G", 1)
|
|
return 1
|
|
|
|
# licht on
|
|
# taste ,
|
|
|
|
elif event1.code == 51:
|
|
robot1.move("L", 1)
|
|
return 1
|
|
|
|
# taste m
|
|
|
|
elif event1.code == 50:
|
|
robot1.cleanup()
|
|
robot2.cleanup()
|
|
return 1
|
|
|
|
|
|
def robot2func(event2):
|
|
|
|
# handgelenk on
|
|
# taste w
|
|
|
|
if event2.code == 17:
|
|
robot2.move("H", 1)
|
|
return 1
|
|
|
|
# handgelenk off
|
|
# taste s
|
|
|
|
if event2.code == 31:
|
|
robot2.move("H", 2)
|
|
return 1
|
|
|
|
# ellenbogen on
|
|
# taste e
|
|
|
|
elif event2.code == 18:
|
|
robot2.move("E", 1)
|
|
return 1
|
|
|
|
# ellenbogen off
|
|
# taste d
|
|
elif event2.code == 32:
|
|
robot2.move("E", 2)
|
|
return 1
|
|
|
|
# arm on
|
|
# taste r
|
|
|
|
elif event2.code == 19:
|
|
robot2.move("A", 1)
|
|
return 1
|
|
|
|
# arm off
|
|
# taste f
|
|
|
|
elif event2.code == 33:
|
|
robot2.move("A", 2)
|
|
return 1
|
|
|
|
# basis off
|
|
# taste x
|
|
|
|
elif event2.code == 45:
|
|
robot2.move("B", 2)
|
|
return 1
|
|
|
|
# basis on
|
|
# taste c
|
|
|
|
elif event2.code == 46:
|
|
robot2.move("B", 1)
|
|
return 1
|
|
|
|
# greifer off
|
|
# taste q
|
|
|
|
elif event2.code == 16:
|
|
robot2.move("G", 2)
|
|
return 1
|
|
|
|
# greifer on
|
|
# taste a
|
|
|
|
elif event2.code == 30:
|
|
robot2.move("G", 1)
|
|
return 1
|
|
|
|
# licht on
|
|
# taste y
|
|
|
|
elif event2.code == 44:
|
|
robot2.move("L", 1)
|
|
return 1
|
|
|
|
# taste v
|
|
|
|
elif event2.code == 47:
|
|
robot2.cleanup()
|
|
return 1
|
|
|
|
|
|
def runInParallel(*fns):
|
|
proc = []
|
|
for fn in fns:
|
|
p = Process(target=fn)
|
|
p.start()
|
|
proc.append(p)
|
|
for p in proc:
|
|
p.join()
|
|
|
|
|
|
def manuell():
|
|
flag1 = 0
|
|
flag2 = 0
|
|
for event in dev.read_loop():
|
|
if event.type == ecodes.EV_KEY:
|
|
if event.value == 1:
|
|
if flag1 == 0:
|
|
flag1 = robot1func(event)
|
|
if not flag1:
|
|
flag1 = 0
|
|
|
|
if flag2 == 0:
|
|
flag2 = robot2func(event)
|
|
if not flag2:
|
|
flag2 = 0
|
|
|
|
if event.value == 0:
|
|
if flag1 == 1:
|
|
if robot1func(event) == 1:
|
|
robot1.reset()
|
|
flag1 = 0
|
|
|
|
if flag2 == 1:
|
|
if robot2func(event) == 1:
|
|
robot2.reset()
|
|
flag2 = 0
|
|
|
|
|
|
#def automatic1():
|
|
robot1.move("G", 1)
|
|
time.sleep(1.5)
|
|
robot1.reset()
|
|
robot1.move("E", 1)
|
|
time.sleep(4)
|
|
robot1.reset()
|
|
robot1.move("B", 2)
|
|
time.sleep(3)
|
|
robot1.reset()
|
|
|
|
automatic2()
|
|
robot2.move("B", 2)
|
|
time.sleep(4)
|
|
robot2.move("E", 2)
|
|
time.sleep(4)
|
|
robot2.reset()
|
|
robot1.move("B", 1)
|
|
time.sleep(3)
|
|
robot1.reset()
|
|
robot1.move("E", 2)
|
|
time.sleep(3.5)
|
|
robot1.reset()
|
|
robot2.move("E", 1)
|
|
time.sleep(3)
|
|
robot2.reset()
|
|
robot2.move("B", 1)
|
|
time.sleep(4)
|
|
robot2.reset()
|
|
robot2.move("E", 2)
|
|
time.sleep(1)
|
|
robot2.reset()
|
|
robot2.move("G", 2)
|
|
time.sleep(1)
|
|
robot2.reset()
|
|
|
|
|
|
#def automatic2():
|
|
robot2.move("E", 2)
|
|
time.sleep(2)
|
|
robot2.reset()
|
|
robot2.move("G", 1)
|
|
time.sleep(1.5)
|
|
robot2.reset()
|
|
robot1.move("G", 2)
|
|
time.sleep(1)
|
|
robot1.reset()
|
|
robot2.move("E", 1)
|
|
time.sleep(4)
|
|
robot2.reset()
|
|
|
|
|
|
manuell()
|
|
#automatic1()
|