joysticktest9.py

import math
import random

leftPort = "COM9"
rightPort = "COM8"
###########################
#to tweak the default voice
Voice="Microsoft David Desktop" #Male US voice 
#Voice="cmu-slt-hsmm" #Default female for MarySpeech
mouth = Runtime.createAndStart("i01.mouth", "LocalSpeech")
#mouth.installComponentsAcceptLicense(Voice)
mouth.setVoice(Voice)
############################
# starting InMoov service
i01 = Runtime.start("i01", "InMoov2")
#Force Arduino to connect (fix Todo)
i01_left = Runtime.createAndStart("i01.left", "Arduino")
i01_left.connect(leftPort)
sleep(3)
i01_right = Runtime.createAndStart("i01.right", "Arduino")
i01_right.connect(rightPort)
sleep(2)
############################
i01_head = runtime.start('i01.head', 'InMoov2Head')
############################

# Servo Config : i01_head_rothead
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_rothead.setPosition(90)
i01_head_rothead.map(0.0,180.0,30.0,150.0)
i01_head_rothead.setInverted(False)
i01_head_rothead.setSpeed(50.0)
i01_head_rothead.setRest(90.0)
i01_head_rothead.setPin(13)
i01_head_rothead.setAutoDisable(True)

# Servo Config : i01_head_neck
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_neck.setPosition(90)
i01_head_neck.map(0.0,180.0,20.0,160.0)
i01_head_neck.setInverted(False)
i01_head_neck.setSpeed(50.0)
i01_head_neck.setRest(90.0)
i01_head_neck.setPin(12)
i01_head_neck.setAutoDisable(True)

# Servo Config : i01_head_jaw
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_jaw.setPosition(0)
i01_head_jaw.map(0.0,180.0,10.0,25.0)
i01_head_jaw.setInverted(False)
i01_head_jaw.setSpeed(100.0)
i01_head_jaw.setRest(0.0)
i01_head_jaw.setPin(26)
i01_head_jaw.setAutoDisable(True)

# Servo Config : i01_head_rollNeck
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_rollNeck.setPosition(90)
i01_head_rollNeck.map(0.0,180.0,60.0,130.0)
i01_head_rollNeck.setInverted(False)
i01_head_rollNeck.setSpeed(50.0)
i01_head_rollNeck.setRest(90.0)
i01_head_rollNeck.setPin(12)
i01_head_rollNeck.setAutoDisable(True)

# Servo Config : i01_head_eyeX
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_eyeX.setPosition(90)
i01_head_eyeX.map(0.0,180.0,60.0,120.0)
i01_head_eyeX.setInverted(False)
i01_head_eyeX.setSpeed(50.0)
i01_head_eyeX.setRest(90.0)
i01_head_eyeX.setPin(22)
i01_head_eyeX.setAutoDisable(True)

# Servo Config : i01_head_eyeY
# sets initial position of servo before moving
# in theory this is the position of the servo when this file was created
i01_head_eyeY.setPosition(90)
i01_head_eyeY.map(0.0,180.0,60.0,120.0)
i01_head_eyeY.setInverted(False)
i01_head_eyeY.setSpeed(50.0)
i01_head_eyeY.setRest(90.0)
i01_head_eyeY.setPin(24)
i01_head_eyeY.setAutoDisable(True)
#################
#We attach this part to the left Arduino
i01_left.attach("i01.head.rothead")
i01_left.attach("i01.head.neck")
i01_left.attach("i01.head.jaw")
i01_left.attach("i01.head.eyeY")
i01_left.attach("i01.head.eyeX")
#We attach this part to the right Arduino
i01_right.attach("i01.head.rollNeck")
############################
# Disable all servo at start
i01_head_neck.disable()
i01_head_rothead.disable()
i01_head_rollNeck.disable()
i01_head_eyeX.disable()
i01_head_eyeY.disable()
i01_head_jaw.disable()
# Start the Joystick service
############################
uberjoy = Runtime.createAndStart("uberjoy", "Joystick")
# Update this to be your controller
uberjoy.setController(2)
uberjoy.startPolling()


def StickXListener(value):
  global i01_head
  threshold = 0.2
  # a scaling factor for the joystick value
  gain = 100
  # many joysticks don't snap back to a true 0 position. 
  # when i let go of the xbox controller joystick, often it snaps back to center
  # and still reads about 0.17
  absValue = math.fabs(value)
  if (absValue < threshold ):
    # below the threshold, stop the servo where it is.
    i01_head_rothead.setSpeed(0)
    i01_head_rothead.moveTo(i01_head_rothead.pos)
    return
  else:
    # set speed to some amount based on the joystick position
    speed = absValue * gain
    i01_head_rothead.setSpeed(speed)
    # set the direction of the movement
    if (value < 0):
      i01_head_rothead.moveTo(i01_head_rothead.min)
    else:
      i01_head_rothead.moveTo(i01_head_rothead.max)
    return

def StickYListener(value):
  global i01_head
  threshold = 0.2
  # a scaling factor for the joystick value
  gain = 100
  # many joysticks don't snap back to a true 0 position. 
  # when i let go of the xbox controller joystick, often it snaps back to center
  # and still reads about 0.17
  absValue = math.fabs(value)
  if (absValue < threshold ):
    # below the threshold, stop the servo where it is.
    i01_head_neck.setSpeed(0)
    i01_head_neck.moveTo(i01_head_neck.pos)
    return
  else:
    # set speed to some amount based on the joystick position
    speed = absValue * gain
    i01_head_neck.setSpeed(speed)
    # set the direction of the movement
    if (value < 0):
      i01_head_neck.moveTo(i01_head_neck.max)
    else:
      i01_head_neck.moveTo(i01_head_neck.min)
    return

def StickZListener(value):
  global i01_head
  threshold = 0.2
  # a scaling factor for the joystick value
  gain = 100
  # many joysticks don't snap back to a true 0 position. 
  # when i let go of the xbox controller joystick, often it snaps back to center
  # and still reads about 0.17
  absValue = math.fabs(value)
  if (absValue < threshold ):
    # below the threshold, stop the servo where it is.
    i01_head_rollNeck.setSpeed(0)
    i01_head_rollNeck.moveTo(i01_head_rollNeck.pos)
    return
  else:
    # set speed to some amount based on the joystick position
    speed = absValue * gain
    i01_head_rollNeck.setSpeed(speed)
    # set the direction of the movement
    if (value < 0):
      i01_head_rollNeck.moveTo(i01_head_rollNeck.min)
    else:
      i01_head_rollNeck.moveTo(i01_head_rollNeck.max)
    return

def StickRXListener(value):
  global i01_head
  threshold = 0.2
  # a scaling factor for the joystick value
  gain = 100
  # many joysticks don't snap back to a true 0 position. 
  # when i let go of the xbox controller joystick, often it snaps back to center
  # and still reads about 0.17
  absValue = math.fabs(value)
  if (absValue < threshold ):
    # below the threshold, stop the servo where it is.
    i01_head_eyeX.setSpeed(0)
    i01_head_eyeX.moveTo(i01_head_eyeX.pos)
    return
  else:
    # set speed to some amount based on the joystick position
    speed = absValue * gain
    i01_head_eyeX.setSpeed(speed)
    # set the direction of the movement
    if (value < 0):
      i01_head_eyeX.moveTo(i01_head_eyeX.max)
    else:
      i01_head_eyeX.moveTo(i01_head_eyeX.min)
    return

def StickRYListener(value):
  global i01_head
  threshold = 0.2
  # a scaling factor for the joystick value
  gain = 100
  # many joysticks don't snap back to a true 0 position. 
  # when i let go of the xbox controller joystick, often it snaps back to center
  # and still reads about 0.17
  absValue = math.fabs(value)
  if (absValue < threshold ):
    # below the threshold, stop the servo where it is.
    i01_head_eyeY.setSpeed(0)
    i01_head_eyeY.moveTo(i01_head_eyeY.pos)
    return
  else:
    # set speed to some amount based on the joystick position
    speed = absValue * gain
    i01_head_eyeY.setSpeed(speed)
    # set the direction of the movement
    if (value < 0):
      i01_head_eyeY.moveTo(i01_head_eyeY.min)
    else:
      i01_head_eyeY.moveTo(i01_head_eyeY.max)
    return

def Stick1Listener(value):
    global i01_head
    i01_head_jaw.moveTo(i01_head_jaw.min)
    sleep(0.2)
    i01_head_jaw.disable()
    
def Stick2Listener(value):
    global i01_head
    i01_head_jaw.moveTo(120)
    sleep(0.2)
    i01_head_jaw.disable()

def Stick3Listener(value):
    global i01_head
    i01_head_jaw.moveTo(70)
    sleep(0.2)
    i01_head_jaw.disable()

def Stick0Listener(value):
    global i01_head
    i01_head_jaw.moveTo(i01_head_jaw.max)
    sleep(0.2)
    i01_head_jaw.disable()             
                           

# The main callback method from the joystick service with all button data as it changes
def onJoystickInput(data):
    print "Joystick Data:" + str(data)
    if (data.id == "x"):
        StickXListener(data.value)
    if (data.id == "y"):
        StickYListener(data.value)
    if (data.id == "rx"):
        StickRXListener(data.value)
    if (data.id == "ry"):
        StickRYListener(data.value)    
    if (data.id == "z"):
        StickZListener(data.value)
    if (data.id == "1"):
        Stick1Listener(data.value)
    if (data.id == "2"):
        Stick2Listener(data.value)
    if (data.id == "3"):
        Stick3Listener(data.value)
    if (data.id == "0"):
        Stick0Listener(data.value)
    if (data.id == "7"):
        Stick7Listener(data.value)
    if (data.id == "6"):
        Stick6Listener(data.value)
    if (data.id == "5"):
        Stick5Listener(data.value)        


# Extra Function
########################################################
def Stick7Listener(value):
  i01.opencv.capture()
  #i01.opencv.addFilter("PyramidDown")
  #i01.opencv.addFilter("FaceDetect")

def Stick6Listener(value):
  #i01.opencv.removeFilter("PyramidDown")
  #i01.opencv.removeFilter("FaceDetect")
  i01.opencv.stopCapture()

def Stick5Listener(value):
  x = (random.randint(1, 12))
  if x == 1:
    i01_mouth.speakBlocking("Did you just try to make me speak?")
    sleep(1)
    i01_head_jaw.disable()
  if x == 2:
    i01_mouth.speakBlocking("Do you regret what you did?")
    sleep(1)
    i01_head_jaw.disable()
  if x == 3:
    i01_mouth.speakBlocking("Nobody gets lucky all the time. Nobody can win all the time. Nobody's a robot. Nobody's perfect.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 4:
    i01_mouth.speakBlocking("People see me on the court only as a superhero, grunting and winning. They think you're a robot, and I'm not.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 5:
    i01_mouth.speakBlocking("What's casual for a robot isn't necessarily what's casual for a human.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 6:
    i01_mouth.speakBlocking("I would love to have a robot at home.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 7:
    i01_mouth.speakBlocking("I am not a robot. I have a heart and I bleed.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 8:
    i01_mouth.speakBlocking("I make mistakes growing up. I'm not perfect; I'm not a robot.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 9:
    i01_mouth.speakBlocking("Every great device, gadget, electric car, and robot would be even greater if batteries didn't suck so badly.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 10:
    i01_mouth.speakBlocking("You probably found 'How to Survive a Robot Uprising' in the humor section. Let's just hope that is where it belongs.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 11:
    i01_mouth.speakBlocking("Everybody's out there wrestling like a robot.")
    sleep(1)
    i01_head_jaw.disable()
  if x == 12:
    i01_mouth.speakBlocking("If you lend your consciousness to someone else, you're a robot.") 
    sleep(1)     
    i01_head_jaw.disable()                          

########################################################
# attach the python service to the published data from the joystick

uberjoy.addInputListener(python)