From 5c294be8df967172f47c904c7ddc81445e7cd151 Mon Sep 17 00:00:00 2001
From: Thomas Muller <tmuller2017@my.fit.edu>
Date: Wed, 8 Sep 2021 22:35:29 +0000
Subject: [PATCH] Real teensy protocol

---
 src/cobot_platform/cobot_platform/move.py | 33 ++++++++++++-----------
 1 file changed, 18 insertions(+), 15 deletions(-)

diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py
index 34e564a..824fba0 100644
--- a/src/cobot_platform/cobot_platform/move.py
+++ b/src/cobot_platform/cobot_platform/move.py
@@ -8,9 +8,9 @@ from serial import Serial
 from enum import Enum
 
 class Servos(Enum):
-    BODY = 0
-    ARM = 1
-    HAND = 2
+    J1 = 'A'
+    J2 = 'B'
+    J3 = 'C'
 
 class CobotMove(Node):
     def __init__(self):
@@ -25,24 +25,27 @@ class CobotMove(Node):
 
         base_topic = 'move/'
 
-        self.create_subscription(Float32, base_topic+'body', self._move_body_cb, rclpy.qos.qos_profile_sensor_data)
-        self.create_subscription(Float32, base_topic+'arm',  self._move_arm_cb,  rclpy.qos.qos_profile_sensor_data)
-        self.create_subscription(Float32, base_topic+'hand', self._move_hand_cb, rclpy.qos.qos_profile_sensor_data)
+        self.create_subscription(Float32, base_topic+'j1', self._move_j1_cb, rclpy.qos.qos_profile_sensor_data)
+        self.create_subscription(Float32, base_topic+'j2',  self._move_j2_cb,  rclpy.qos.qos_profile_sensor_data)
+        self.create_subscription(Float32, base_topic+'j3', self._move_j3_cb, rclpy.qos.qos_profile_sensor_data)
 
-    def _move_body_cb(self, msg):
-        self._move_servo(Servos.BODY, msg.data)
+        self._serial.write(b'STA0.0.1\n')
+        print(self._serial.readline())
 
-    def _move_arm_cb(self, msg):
-        self._move_servo(Servos.ARM, msg.data)
+    def _move_j1_cb(self, msg):
+        self._move_servo(Servos.J1, msg.data)
 
-    def _move_hand_cb(self, msg):
-        self._move_servo(Servos.HAND, msg.data)
+    def _move_j2_cb(self, msg):
+        self._move_servo(Servos.J2, msg.data)
+
+    def _move_j3_cb(self, msg):
+        self._move_servo(Servos.J3, msg.data)
 
     def _move_servo(self, serv_num, pos):
-        cmd = 'SV'
+        cmd = 'MT'
         #  self._buf[serv_num] = pos
-        self._logger.info('COBOT: Servo {serv_num} to {pos}')
-        self._serial.write(f'{cmd}V{serv_num}P{pos}\n'.encode('UTF-8'))
+        self._serial.write(f'{cmd}A{int(pos * 10000)}B0C0D0E0F0\n'.encode('UTF-8'))
+        print(self._serial.readline())
 
     # def _push ?