#!/usr/bin/env python

#import the USB abd time libraries
#!/usr/bin/env python
import skywriter
import signal
import time
import usb.core, usb.util, time

position = False

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

#Check to see if arm is detected
if RoboArm is None:
     raise ValueError("Arm not found")

#Create a variable for duration
Duration=1

#Define a procedure to execute each movement
def MoveArm(Duration, ArmCmd):
     #start the movement
     RoboArm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)
     #stop the movement atfer specified duration
     time.sleep(Duration)
     ArmCmd=(0,0,0)
     RoboArm.ctrl_transfer(0x40,6,0x100,0,ArmCmd,1000)

###DOUBLE TAP NORTH FOR ELBOW UP / DOUBLE TAP SOUTH / DOWN ###
###DOUBLE TAP EAST = OPEN GRIP, WEAT = CLOSE GRIP ###          
@skywriter.touch(repeat_rate=1) ###Raise / lower the wrist
def touch(position2):
     if position2 == 'north':
          MoveArm(1,[16,0,0]) #elbow up
          print "Elbow Up"
     elif position2 == 'south':
          MoveArm(1,[32,0,0]) #elbow down
     elif position2 == 'west':
          MoveArm(1,[1,0,0]) #grip close
     elif position2 == 'east':
          MoveArm(1,[2,0,0]) ##grip open
     elif position2 == 'center':
          MoveArm(1,[0,0,1]) #light on
     
   
###TOUCH EAST / WEST TO MOVE BASE LEFT OR RIGHT###
###TOUCH NORTH / SOUTH TO RASIE / LOWER THE SHOULDER###          
@skywriter.tap(repeat_rate=1)
def tap(position1):
     print('Tap!', position1)
     if position1 == 'north':
          print "Shoulder Up!"
          MoveArm(1,[64,0,0]) #shoulder up
     elif position1 == 'south':
          print "Shoulder Up!"
          MoveArm(1,[128,0,0]) #shoulder down
     elif position1 == 'west':
          print "Base Left!"
          MoveArm(1,[0,1,0]) #rotate base anti-clockwise
     elif position1 == 'east':
          print "Base Right!"
          MoveArm(1,[0,2,0]) #rotate base clockwisez


     

          


@skywriter.flick() ###use a flick to open and close the gripper
def flick(start,finish):
  print('Got a flick!', start, finish)
  if start == 'south' and finish == 'north':
          MoveArm(1,[2,0,0])
  elif start == 'west' and finish == 'east':
          MoveArm(1,[1,0,0])        

     
signal.pause()     

#Give arm some commands
#MoveArm(1,[0,1,0]) #rotate base anti-clockwise
#MoveArm(1,[0,2,0]) #rotate base clockwise
#MoveArm(1,[64,0,0]) #shoulder up
#MoveArm(1,[128,0,0]) #shoulder down
#MoveArm(1,[16,0,0]) #elbow up
#MoveArm(1,[32,0,0]) #elbow down
#MoveArm(1,[4,0,0]) #wrist up
#MoveArm(1,[8,0,0]) #wrist down
#MoveArm(1,[2,0,0]) #grip open
#MoveArm(1,[1,0,0]) #grip closed
#MoveArm(1,[0,0,1]) #light on
#MoveArm(1,[0,0,0]) #light off