open_hand.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # coding:utf-8
3 
4 import time
5 import serial
6 import struct
7 
8 
9 class RS304MD(object):
10  def __init__(self):
11  SERIAL_BAUDRATE = 115200
12  SERIAL_BYTESIZE = serial.EIGHTBITS
13  SERIAL_PARITY = serial.PARITY_NONE
14  SERIAL_STOPBIT = serial.STOPBITS_ONE
15  SERIAL_TIMEOUT = 1
16  self.ser = serial.Serial("/dev/ttyUSB0", SERIAL_BAUDRATE, SERIAL_BYTESIZE, SERIAL_PARITY, SERIAL_STOPBIT,
17  SERIAL_TIMEOUT)
18 
19  def __del__(self):
20  self.ser.close()
21 
22  def __bytecreateid(self, servo_id):
23  return [0xFA, 0xAF, servo_id]
24 
25  def __checksum(self, checklist):
26  sum = 0
27  for i in range(2, len(checklist)):
28  sum ^= checklist[i]
29  checklist.append(sum)
30  return checklist
31 
32  def __write(self, servolist):
33  self.ser.write("".join(map(chr, servolist)))
34 
35  def __requestStatus(self, servo_id):
36  a = self.__bytecreateid(servo_id)
37  a.extend([0x09, 0x00, 0x00, 0x01])
38  self.__write(self.__checksum(a))
39  time.sleep(0.001)
40 
41  def setAngle(self, servo_id, set_angle):
42  angle = max(-150.0, min(150.0, set_angle))
43  angle = int(angle * 10)
44  a = self.__bytecreateid(servo_id)
45  a.extend([0x00, 0x1E, 0x02, 0x01, angle & 0xFF, (angle & 0xFF00) >> 8])
46  self.__write(self.__checksum(a))
47 
48  def setTorque(self, servo_id, onoff):
49  a = self.__bytecreateid(servo_id)
50  a.extend([0x00, 0x24, 0x01, 0x01, int(onoff)])
51  self.__write(self.__checksum(a))
52 
53  def readAngle(self, servo_id):
54  self.__requestStatus(servo_id)
55  b = self.ser.read(26)[7:9]
56  return struct.unpack("<h", b)[0] / 10.0
57 
58  def readCurrent(self, servo_id):
59  self.__requestStatus(servo_id)
60  b = self.ser.read(26)[13:15]
61  return struct.unpack("<h", b)[0]
62 
63 
64 class NekonoteEEF(object):
65  def __init__(self):
66  self.reso = rospy.get_param("~reso", 2.0)
67  self.rs = RS304MD()
68  sub = rospy.Subscriber("nekonote_eef", Float32, self.callback)
69  self.s = rospy.Service("karaage_tsukamuzo", HandGrab, self.service_handle)
70  self.service = rospy.Service("ganbaruzoi", CurrentLoad, self.handlehandle)
71  self.rs.setTorque(1, True)
72  time.sleep(2)
73  rospy.spin()
74 
75  def handlehandle(self, req):
76  # self.rs.setAngle(1, req.grab_angle)
77  # time.sleep(2)
78  # self.rs.setAngle(1, 0)
79  # time.sleep(2)
80  # angle = self.rs.readAngle(1)
81  # if angle < 5.0:
82  # return hand_grabResponse(False)
83  # else:
84  # return hand_grabResponse(True)
85  time.sleep(1)
86  current = self.rs.readCurrent(1)
87  return CurrentLoadResponse(current)
88 
89  def service_handle(self, req):
90  """
91  :type req: HandGrabRequest
92  :param req:
93  :return: HandGrabResponse
94  """
95  self.rs.setAngle(1, req.hand_angle)
96  time.sleep(2)
97  i = req.hand_angle
98  for j in range(int(req.hand_angle * self.reso), 0, -1):
99  # while self.rs.readCurrent(1) < 300:
100  k = j / self.reso
101  self.rs.setAngle(1, k)
102  time.sleep(0.001)
103  if self.rs.readCurrent(1) > 400:
104  time.sleep(1)
105  return HandGrabResponse(True)
106  time.sleep(1)
107  return HandGrabResponse(False)
108 
109  def callback(self, msg):
110  self.rs.setAngle(1, msg.data)
111  time.sleep(0.01)
112 
113  def read(self):
114  return self.rs.readCurrent(1)
115 
116 
117 if __name__ == '__main__':
118  rs = RS304MD()
119 
120  for i in range(1,6):
121  rs.setTorque(i, True)
122  time.sleep(0.1)
123 
124  for i in range(1,6):
125  rs.setAngle(i, 0)
126  time.sleep(0.1)
127 
128  for i in range(1,6):
129  for j in range(0,21):
130  rs.setAngle(i, -j)
131  time.sleep(0.1)
132 
133  # rs.setAngle(0,-20)
134  # time.sleep(0.1)
135  # rs.setAngle(1,-20)
136  # time.sleep(0.1)
137  # rs.setAngle(2,-40)
138  # time.sleep(0.1)
139  # rs.setAngle(3,-40)
140  # time.sleep(0.1)
141  # rs.setAngle(4,-20)
142  # time.sleep(0.1)
143  # rs.setAngle(5,-20)
144  # time.sleep(0.1)
145  #
146 
def __del__(self)
Definition: open_hand.py:19
def setTorque(self, servo_id, onoff)
Definition: open_hand.py:48
def readCurrent(self, servo_id)
Definition: open_hand.py:58
def __requestStatus(self, servo_id)
Definition: open_hand.py:35
def __init__(self)
Definition: open_hand.py:10
def service_handle(self, req)
Definition: open_hand.py:89
def __write(self, servolist)
Definition: open_hand.py:32
def callback(self, msg)
Definition: open_hand.py:109
def setAngle(self, servo_id, set_angle)
Definition: open_hand.py:41
def __bytecreateid(self, servo_id)
Definition: open_hand.py:22
def __init__(self)
Definition: open_hand.py:65
def __checksum(self, checklist)
Definition: open_hand.py:25
def handlehandle(self, req)
Definition: open_hand.py:75
def readAngle(self, servo_id)
Definition: open_hand.py:53


raspigibbon_utils
Author(s): Daisuke Sato , Shota Hirama
autogenerated on Mon Jun 10 2019 14:25:23