From fb576de8344aee47c233f66fe4361407a1cae893 Mon Sep 17 00:00:00 2001
From: ValsCSGO <valscsgo@gmail.com>
Date: Fri, 8 Oct 2021 17:47:53 -0400
Subject: [PATCH] Get encoder values all the time

---
 src/cobot_platform/cobot_platform/move.py | 27 +++++++++++++++++++++--
 src/joystickmove/joystickmove/move.py     |  7 ------
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py
index bdba73b..cb7900e 100644
--- a/src/cobot_platform/cobot_platform/move.py
+++ b/src/cobot_platform/cobot_platform/move.py
@@ -21,6 +21,8 @@ class CobotMove(Node):
         # self._buf = [90, 90, 90] # 3 servos
         # self._prev_buf = [90, 90, 90]
 
+        self.write_lock = False
+
         base_topic = 'move/'
 
         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')
         print(self._serial.readline())
 
+        self._timer = self.create_timer(.001, self._get_encoders)
+
     def _move_j1_cb(self, msg):
         self._cur[0] = int(msg.data / pi / 2. * 8000 * 4)
         self._move_servo()
@@ -70,6 +74,8 @@ class CobotMove(Node):
         self._move_servo()
 
     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'))
 
         line = self._serial.readline().decode('UTF-8')
@@ -88,14 +94,31 @@ class CobotMove(Node):
 
         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_j2.publish(Float32(data=b))
         self.s_j3.publish(Float32(data=c))
         self.s_j4.publish(Float32(data=d))
         self.s_j5.publish(Float32(data=e))
         self.s_j6.publish(Float32(data=f))
-    #    self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
-    # def _push ?
+
 
 def main(args=None):
     rclpy.init(args=args)
diff --git a/src/joystickmove/joystickmove/move.py b/src/joystickmove/joystickmove/move.py
index 94f69d4..f1b9215 100644
--- a/src/joystickmove/joystickmove/move.py
+++ b/src/joystickmove/joystickmove/move.py
@@ -63,13 +63,6 @@ class JoyMove(Node):
         req = Trigger.Request()
         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):
         axes = msg.axes
         buttons = msg.buttons