Home Programming

Discussion

Left ArrowBack to discussions page
bcastetsbcastets Vacuum Beta tester Posts: 156 Handy
edited July 2019 in Programming
I would like to share with you a little program I made to control UR robot using a joystick connected on a PC.

Here is how it works:

A python program is running on my pc. It catchs the position of joystick axis and write this position in UR robot modbus server input register.
On the robot, a program is running. It calculates the next position of the robot depending on the position of the joystick and order the next move.

Python script and ur program are available on github:
https://github.com/castetsb/urJoystickControl

#Import libraries
##################
import time
import pygame
from pyModbusTCP.client import ModbusClient
import keyboard  # using module keyboard

#Initialize variables
##################
executionTime=0
MODBUS_SERVER_IP="10.20.1.108"

#Process initialization
##################

#Joystick
pygame.joystick.init()
joy=pygame.joystick.Joystick(0)
joy.init()
pygame.display.init()

#communication
# TCP auto connect on first modbus request
c = ModbusClient(host=MODBUS_SERVER_IP, port=502, auto_open=True)
c.host(MODBUS_SERVER_IP)
c.port(502)
# managing TCP sessions with call to c.open()/c.close()
c.open()

#Functions definition
def rescaleAxis(axisPosition,axisIniPosition):
  #Rescale axis position according to its initial resting position.
  #
  #Parameters:
  # axisposition: axis position of the joystick. It should be a value between -1 and 1.
  # axisIniPosition: axis position when the joystick is at rest. It should be a value between -1 and 1.
  #
  #Return:
  # rescaledPosition: axis position between -1 and 1 with a value equal to 0 when the joystick is at rest.
  rescaledPosition=0
  
  lowSegmentLen=1+axisIniPosition
  upperSegmentLen=1-axisIniPosition
  
  if axisPosition<axisIniPosition:
    rescaledPosition=((axisPosition-axisIniPosition)/lowSegmentLen)
  else:
    rescaledPosition=((axisPosition-axisIniPosition)/upperSegmentLen)
  return rescaledPosition

##################
##################
#Main program
##################
##################
#This program get the joystick position and send it to UR robot MODBUS server.
##################

#initialize Joystick position
pygame.event.pump()
axis0Ini=joy.get_axis(0)
axis1Ini=joy.get_axis(1)
axis2Ini=joy.get_axis(2)
axis3Ini=joy.get_axis(3)

while True:
  try:
    if keyboard.is_pressed('q'):  # if key 'q' is pressed 
      print('You Pressed q Key! The program stopped.')
      break
    else:
      #get joystick position
      pygame.event.pump()
      
      #right joystick
      
      #-1 for left, 1 for right
      axis0=joy.get_axis(0)
      axis0=rescaleAxis(axis0,axis0Ini)

      #-1 up 1 down
      axis1=joy.get_axis(1)
      axis1=rescaleAxis(axis1,axis1Ini)
      
      #left joystick
      
      #-1 for left, 1 for right
      axis2=joy.get_axis(2)
      axis2=rescaleAxis(axis2,axis2Ini)
      
      #-1 up 1 down
      axis3=joy.get_axis(3)
      axis3=rescaleAxis(axis3,axis3Ini)
      
      print([round(axis0,2),round(axis1,2),round(axis2,2),round(axis3,2)])
      #Send axis position to Modbus server registers 128, 129, 130 and 131
      if c.write_multiple_registers(128, [int((axis0+1)*100),int((axis1+1)*100),int((axis2+1)*100),int((axis3+1)*100)]):
        pass
      else:
        print("Error")
  except:
      break

Here is the modbus server setup on UR robot:




Comments

  • bcastetsbcastets Vacuum Beta tester Posts: 156 Handy
    Here is a video showing a UR robot controlled with this program:


  • tdominotdomino Unconfirmed Posts: 2 Recruit
    hi, please can i have more explications about your program, your set up, and if your program can be uploaded in a polyscope 3.5 version ?

    i try to put the program in my UR5 but there are some problems :
    my set up : UR5 (192.178.1.2) + polyscope 3.5

    PC(ubuntu)  + xbox360 joystick + your python program

    i have a problem when i start the python program :
    Traceback (most recent call last):
      File "urJoystickControl.py", line 64, in <module>
        axis3Ini=joy.get_axis(3)
    pygame.error: Invalid joystick axis

    when i start the UR program, the UR5 move alone ..

  • bcastetsbcastets Vacuum Beta tester Posts: 156 Handy
    This is a problem with your joystick and the integration with pygame Joystick (https://www.pygame.org/docs/ref/joystick.html).
    Looking at the error message it looks like your joystick does not have axis 3.
    I would recommend that you start making  a simple python program trying to get and display axis position. This way you can identify the axis of your joystick and adjust my program is needed.
    The program should run under 3.5, some little adjustment maybe required.

    It is a side comment but after this project, I learned that using speedL script command in the UR program would be better to have a fluid motion. With my program the robot progress step by step.

  • tdominotdomino Unconfirmed Posts: 2 Recruit
    ok thanks i will see.

    can you developp your comment : " speedL script command in the UR program would be better to have a fluid motion." thanks a lot

  • bcastetsbcastets Vacuum Beta tester Posts: 156 Handy
    MoveJ and MoveL always stop want to motion is completed. Those command are use to request a position.
    SpeedJ and SpeedL are used to request a speed. If you request speed the robot does not stop between each commands.
    There are more details about this in UR script manual.
Sign In or Register to comment.
Left ArrowBack to discussions page