#!/usr/bin/python

# ROBOT ARM CONTROL PROGRAM
# import the USB and Time libraries into Python
import usb.core, usb.util, time, sys
from getch import getch

# Allocate the name 'RoboArm' to the USB device
RoboArm = usb.core.find(idVendor=0x1267, idProduct=0x0000)

# Check if the arm is detected and warn if not
if RoboArm is None:
	raise ValueError("Arm not found")
lights = 0
# Create a variable for duration
delay = 0.05
timeout = 1000

# Define a procedure to execute each movement
def MoveArm(Duration, ArmCmd):
	# Start the movement
	RoboArm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,timeout)
	# Stop the movement after waiting specified duration
	time.sleep(Duration)
	ArmCmd=[0,0,lights]
	RoboArm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,timeout)

print('\n============= ROBOTARM-CONTROL =============\n')
print('           -> Beenden mit \'x\' <-\n')
print('links/rechts          : \'a\' \'s\'')
print('Schulter hoch/runter  : \'e\' \'d\'')
print('Ellenbogen hoch/runter: \'r\' \'f\'')
print('Handgelenk hoch/runter: \'t\' \g\'')
print('Zange zu/auf          : \'q\' \'w\'')
print('Licht an/aus (toggle) : \'l\'\n')

while 1:
   c = getch()
   if c == 'x':
        print('Ende!\n')
        break
   elif c == 'a':
        print('rotiere links')        
        MoveArm(delay,[0,2,lights])       
   elif c == 's':
        print('rotiere rechts')
        MoveArm(delay,[0,1,lights])       
   elif c == 'e':
        print('Schulter hoch')
        MoveArm(delay,[64,0,lights])     
   elif c == 'd':
        print('Schulter runter')
        MoveArm(delay,[128,0,lights])        
   elif c == 'r':
        print('Ellenbogen hoch')
        MoveArm(delay,[16,0,lights])       
   elif c == 'f':
        print('Ellenbogen runter')
        MoveArm(delay,[32,0,lights])   
   elif c == 't':
        print('Handgelenk hoch')
        MoveArm(delay,[4,0,lights])      
   elif c == 'g':
        print('Handgelenk runter')
        MoveArm(delay,[8,0,lights])       
   elif c == 'q':
        print('Zange schliessen')
        MoveArm(delay,[1,0,lights])       
   elif c == 'w':
        print('Zange oeffnen')
        MoveArm(delay,[2,0,lights])       
   elif c == 'l':
        if lights == 1:
                lights = 0
                print('Licht aus')
                MoveArm(delay,[0,0,lights])              
        elif lights == 0:
                lights = 1
                print('Licht an')
                MoveArm(delay,[0,0,lights])                    
   else:
        print('weiss auch nicht ...')