daily pastebin goal
37%
SHARE
TWEET

Untitled

a guest Oct 20th, 2018 72 Never
Upgrade to PRO!
ENDING IN00days00hours00mins00secs
  1. #Controlling Drawing Arm with mouse
  2. import math
  3. import time
  4. import math
  5. import threading
  6. import sys
  7. import numpy
  8. import os
  9. import time
  10. import ctypes
  11.  
  12. if os.name == 'nt':
  13.     import msvcrt
  14.     def getch():
  15.         return msvcrt.getch().decode()
  16. else:
  17.     import sys, tty, termios
  18.     fd = sys.stdin.fileno()
  19.     old_settings = termios.tcgetattr(fd)
  20.     def getch():
  21.         try:
  22.             tty.setraw(sys.stdin.fileno())
  23.             ch = sys.stdin.read(1)
  24.         finally:
  25.             termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
  26.         return ch
  27.  
  28. import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library
  29.  
  30. # DynamixelSDK parameter
  31. PROTOCOL_VERSION            = 1                             # See which protocol version is used in the Dynamixel
  32. BAUDRATE                    = 1000000    #57600
  33. DEVICENAME                  = "/dev/ttyUSB0".encode('utf-8')
  34. TORQUE_ENABLE               = 1                             # Value for enabling the torque
  35. TORQUE_DISABLE              = 0                             # Value for disabling the torque position value is out of movable
  36. ESC_ASCII_VALUE             = 0x1b
  37. COMM_SUCCESS                = 0                             # Communication Success result value
  38. COMM_TX_FAIL                = -1001                         # Communication Tx Failed
  39. ADDR_GOAL_POS               = 30
  40. ADDR_TORQUE_ENABLE          = 24
  41. ADDR_MOVING_SPEED           = 32
  42. ADDR_PRESENT_POS            = 36
  43. ADDR_FSR_1                  = 26
  44. ADDR_FSR_2                  = 28
  45. ADDR_FSR_3                  = 30
  46. ADDR_FSR_4                  = 32
  47. ADDR_FSR_X                  = 34
  48. ADDR_FSR_Y                  = 35
  49. LEN_GOAL_POSITION           = 2
  50.  
  51. # Initialize ids
  52. motor_ids = range(1,4)
  53. fsr_id = 112
  54. # Initialize PortHandler and PacketHandler Structs
  55. port_num = dynamixel.portHandler(DEVICENAME)
  56. dynamixel.packetHandler()
  57.  
  58. # Initialize Groupsyncwrite and groupBulkRead instance
  59. group_num = dynamixel.groupBulkRead(port_num, PROTOCOL_VERSION)
  60. groupwrite = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_GOAL_POS, LEN_GOAL_POSITION)
  61.  
  62. dxl_comm_result = COMM_TX_FAIL                              # Communication result
  63. dxl_error = 0                                               # Dynamixel error
  64. dxl_present_position = 0                                    # Present position
  65.  
  66. # Open port
  67. if dynamixel.openPort(port_num):
  68.     print("Succeeded to open the port!")
  69. else:
  70.     print("Failed to open the port!")
  71.     print("Press any key to terminate...")
  72.     getch()
  73.     quit()
  74.  
  75. # Set port baudrate
  76. if dynamixel.setBaudRate(port_num, BAUDRATE):
  77.     print("Succeeded to change the baudrate!")
  78. else:
  79.     print("Failed to change the baudrate!")
  80.     print("Press any key to terminate...")
  81.     getch()
  82.     quit()
  83.  
  84. def enable_torque(ids):
  85.     for id in ids:
  86.         dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
  87.         dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
  88.         dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
  89.         if dxl_comm_result != COMM_SUCCESS:
  90.             print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
  91.         elif dxl_error != 0:
  92.             print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
  93.  
  94.  
  95. atos= 4096/360
  96. angle1=int(22.5*atos)+2048
  97. angle2=int(45*atos)+2048
  98. angle3=int(22.5*atos)+2048
  99. angle4=int(60*atos)+2048
  100. angle5=int(120*atos)+2048
  101. angle6=int(60*atos)+2048
  102.  
  103. # Add parameter storage for Dynamixel present position value
  104. for id in motor_ids:
  105.     dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION)).value
  106.     if dxl_addparam_result != 1:
  107.         print("[ID:%03d] groupBulkRead addparam failed" % (id))
  108.         quit()
  109.  
  110. dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupBulkReadAddParam(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION)).value
  111. if dxl_addparam_result != 1:
  112.     print("[ID:%03d] groupBulkRead addparam failed" % (fsr_id))
  113.     quit()
  114.  
  115. def set_moving_speed(ids, speeds):
  116.     for id, speed in zip(ids,speeds):
  117.         dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MOVING_SPEED, speed)
  118.         dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
  119.         dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
  120.         if dxl_comm_result != COMM_SUCCESS:
  121.             print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
  122.         elif dxl_error != 0:
  123.             print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
  124.  
  125.  
  126. def sync_write(ids, positions):
  127.     for id, pos in zip(ids, positions):
  128.         # Add dxl goal position value to the Syncwrite storage
  129.         addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite, id, pos, LEN_GOAL_POSITION)).value
  130.         if addparam_result != 1:
  131.             print("[ID:%03d] groupSyncWrite addparam failed" % (id))
  132.             quit()
  133.     # Syncwrite goal position
  134.     dynamixel.groupSyncWriteTxPacket(groupwrite)
  135.     # Clear syncwrite parameter storage
  136.     dynamixel.groupSyncWriteClearParam(groupwrite)
  137.  
  138.  
  139. def slideloop():
  140.     file = open('y_zmp.txt', 'w')
  141.     set_moving_speed(motor_ids, [150, 300, 150])
  142.     sync_write(motor_ids, [2048, 2048, 2048])
  143.     time.sleep(1)
  144.     while True:
  145.         pos = []
  146.         # Bulkread present position and moving status
  147.         dynamixel.groupBulkReadTxRxPacket(group_num)
  148.         dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
  149.         for id in motor_ids:
  150.             dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION)).value
  151.             pos.append(dynamixel.groupBulkReadGetData(group_num, id, ADDR_PRESENT_POS, LEN_GOAL_POSITION))
  152.         dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupBulkReadIsAvailable(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION)).value
  153.         pos.append(dynamixel.groupBulkReadGetData(group_num, fsr_id, ADDR_FSR_Y, LEN_GOAL_POSITION))
  154.         print pos
  155.         y_zmp = pos[-1]
  156.         file.write(str(y_zmp)+ "\n")
  157.         if y_zmp <= 65:
  158.             angles = [2730, 3413, 2730]
  159.             sync_write(motor_ids, angles)
  160.  
  161. slide = threading.Thread(name='slideloop', target=slideloop)
  162. slide.start()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top