Get encoder values all the time

This commit is contained in:
John Farrell 2021-10-08 17:47:53 -04:00
parent ee0ba4cb33
commit fb576de834
Signed by: john
GPG key ID: 10543A0DA2EC1E12
2 changed files with 25 additions and 9 deletions

View file

@ -21,6 +21,8 @@ class CobotMove(Node):
# self._buf = [90, 90, 90] # 3 servos # self._buf = [90, 90, 90] # 3 servos
# self._prev_buf = [90, 90, 90] # self._prev_buf = [90, 90, 90]
self.write_lock = False
base_topic = 'move/' base_topic = 'move/'
self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data) self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
@ -45,6 +47,8 @@ class CobotMove(Node):
self._serial.write(b'STA0.0.1\n') self._serial.write(b'STA0.0.1\n')
print(self._serial.readline()) print(self._serial.readline())
self._timer = self.create_timer(.001, self._get_encoders)
def _move_j1_cb(self, msg): def _move_j1_cb(self, msg):
self._cur[0] = int(msg.data / pi / 2. * 8000 * 4) self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
self._move_servo() self._move_servo()
@ -70,6 +74,8 @@ class CobotMove(Node):
self._move_servo() self._move_servo()
def _move_servo(self): def _move_servo(self):
self.write_lock = True
self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8')) self._serial.write(f'MTA{self._cur[0]}B{self._cur[1]}C{self._cur[2]}D{self._cur[3]}E{self._cur[4]}F{self._cur[5]}\n'.encode('UTF-8'))
line = self._serial.readline().decode('UTF-8') line = self._serial.readline().decode('UTF-8')
@ -88,14 +94,31 @@ class CobotMove(Node):
self._joint_pub.publish(self._joint_state) self._joint_pub.publish(self._joint_state)
self.write_lock = False
def _get_encoders(self):
if self.write_lock:
return
self._serial.write(f'JP\n'.encode('UTF-8'))
line = self._serial.readline().decode('UTF-8')
blocks = re.findall(r'[ABCDEF]-*\d+', line)
a = float(blocks[0][1:]) / 8000. / 4. * pi * 2.
b = float(blocks[1][1:]) / 10000 / 4. * pi * 2.
c = float(blocks[2][1:]) / 10000. * pi * 2.
d = float(blocks[3][1:]) / 2000. * pi * 2.
e = float(blocks[4][1:]) / 2000. * pi * 2.
f = float(blocks[5][1:]) / 2000. * pi * 2.
self.s_j1.publish(Float32(data=a)) self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b)) self.s_j2.publish(Float32(data=b))
self.s_j3.publish(Float32(data=c)) self.s_j3.publish(Float32(data=c))
self.s_j4.publish(Float32(data=d)) self.s_j4.publish(Float32(data=d))
self.s_j5.publish(Float32(data=e)) self.s_j5.publish(Float32(data=e))
self.s_j6.publish(Float32(data=f)) self.s_j6.publish(Float32(data=f))
# self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
# def _push ?
def main(args=None): def main(args=None):
rclpy.init(args=args) rclpy.init(args=args)

View file

@ -63,13 +63,6 @@ class JoyMove(Node):
req = Trigger.Request() req = Trigger.Request()
self.servo_client.call_async(req) self.servo_client.call_async(req)
'''
ps = PlanningScene
ps.world = PlanningSceneWorld
ps.is_diff = True
self.collision_pub.publish(ps)
'''
def _joy_cb(self, msg): def _joy_cb(self, msg):
axes = msg.axes axes = msg.axes
buttons = msg.buttons buttons = msg.buttons