fix bug: mybuddy data analysis

Author: anla-xu

demo/drag_trial_teaching_mubuddy.py

@@ -0,0 +1,208 @@
+import time
+import os
+import sys
+import termios
+import tty
+import threading
+import json
+import serial
+import serial.tools.list_ports
+
+from pymycobot import MyBuddy
+
+
+port: str
+mc: MyBuddy
+sp: int = 80
+
+
+def setup():
+    print("")
+    global port, mc
+    plist = list(serial.tools.list_ports.comports())
+    idx = 1
+    for port in plist:
+        print("{} : {}".format(idx, port))
+        idx += 1
+
+    _in = input("\nPlease input 1 - {} to choice:".format(idx - 1))
+    port = str(plist[int(_in) - 1]).split(" - ")[0].strip()
+    print(port)
+    print("")
+
+    baud = 115200
+    _baud = input("Please input baud(default:115200):")
+    try:
+        baud = int(_baud)
+    except Exception:
+        pass
+    print(baud)
+    print("")
+
+    DEBUG = False
+    f = input("Wether DEBUG mode[Y/n]:")
+    if f in ["y", "Y", "yes", "Yes"]:
+        DEBUG = True
+    # mc = MyCobot(port, debug=True)
+    mc = MyBuddy(port, baud, debug=DEBUG)
+
+
+class Raw(object):
+    """Set raw input mode for device"""
+
+    def __init__(self, stream):
+        self.stream = stream
+        self.fd = self.stream.fileno()
+
+    def __enter__(self):
+        self.original_stty = termios.tcgetattr(self.stream)
+        tty.setcbreak(self.stream)
+
+    def __exit__(self, type, value, traceback):
+        termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty)
+
+
+class Helper(object):
+    def __init__(self) -> None:
+        self.w, self.h = os.get_terminal_size()
+
+    def echo(self, msg):
+        print("\r{}".format(" " * self.w), end="")
+        print("\r{}".format(msg), end="")
+
+
+class TeachingTest(Helper):
+    def __init__(self, mycobot) -> None:
+        super().__init__()
+        self.mc = mycobot
+        self.recording = False
+        self.playing = False
+        self.record_list = []
+        self.record_t = None
+        self.play_t = None
+
+    def record(self):
+        self.record_list = []
+        self.recording = True
+
+        def _record():
+            start_t = time.time()
+
+            while self.recording:
+                angles_1 = self.mc.get_encoders(1)
+                angles_2 = self.mc.get_encoders(2)
+                if angles_1 and angles_2:
+                    self.record_list.append([angles_1,angles_2])
+                    time.sleep(0.1)
+                    print("\r {}".format(time.time() - start_t), end="")
+
+        self.echo("Start recording.")
+        self.record_t = threading.Thread(target=_record, daemon=True)
+        self.record_t.start()
+
+    def stop_record(self):
+        if self.recording:
+            self.recording = False
+            self.record_t.join()
+            self.echo("Stop record")
+
+    def play(self):
+        self.echo("Start play")
+        for angles in self.record_list:
+            # print(angles)
+            self.mc.set_encoders(angles[0], 80)
+            self.mc.set_encoders(angles[1], 80)
+            time.sleep(0.1)
+        self.echo("Finish play")
+
+    def loop_play(self):
+        self.playing = True
+
+        def _loop():
+            len_ = len(self.record_list)
+            i = 0
+            while self.playing:
+                idx_ = i % len_
+                i += 1
+                self.mc.set_encoders(self.record_list[idx_], 80)
+                time.sleep(0.1)
+
+        self.echo("Start loop play.")
+        self.play_t = threading.Thread(target=_loop, daemon=True)
+        self.play_t.start()
+
+    def stop_loop_play(self):
+        if self.playing:
+            self.playing = False
+            self.play_t.join()
+            self.echo("Stop loop play.")
+
+    def save_to_local(self):
+        if not self.record_list:
+            self.echo("No data should save.")
+            return
+
+        with open(os.path.dirname(__file__) + "/record.txt", "w") as f:
+            json.dump(self.record_list, f, indent=2)
+            self.echo("save dir:  {}".format(os.path.dirname(__file__)))
+
+    def load_from_local(self):
+
+        with open(os.path.dirname(__file__) + "/record.txt", "r") as f:
+            try:
+                data = json.load(f)
+                self.record_list = data
+                self.echo("Load data success.")
+            except Exception:
+                self.echo("Error: invalid data.")
+
+    def print_menu(self):
+        print(
+            """\
+        \r q: quit
+        \r r: start record
+        \r c: stop record
+        \r p: play once
+        \r P: loop play / stop loop play
+        \r s: save to local
+        \r l: load from local
+        \r f: release mycobot
+        \r----------------------------------
+            """
+        )
+
+    def start(self):
+        self.print_menu()
+
+        while not False:
+            with Raw(sys.stdin):
+                key = sys.stdin.read(1)
+                if key == "q":
+                    break
+                elif key == "r":  # recorder
+                    self.record()
+                elif key == "c":  # stop recorder
+                    self.stop_record()
+                elif key == "p":  # play
+                    self.play()
+                elif key == "P":  # loop play
+                    if not self.playing:
+                        self.loop_play()
+                    else:
+                        self.stop_loop_play()
+                elif key == "s":  # save to local
+                    self.save_to_local()
+                elif key == "l":  # load from local
+                    self.load_from_local()
+                elif key == "f":  # free move
+                    self.mc.release_all_servos()
+                    self.echo("Released")
+                else:
+                    print(key)
+                    continue
+
+
+if __name__ == "__main__":
+    setup()
+    recorder = TeachingTest(mc)
+    recorder.start()

pymycobot/Interface.py

@@ -120,7 +120,7 @@ def get_angles(self, id):
         Return:
             list: A float list of all degree.
         """
-        return self._mesg(ProtocolCode.GET_ANGLES, id)
+        return self._mesg(ProtocolCode.GET_ANGLES, id, has_reply=True)
 
     def send_angle(self, id, joint, angle, speed):
         """Send one degree of joint to robot arm.

pymycobot/common.py

@@ -206,23 +206,23 @@ def _process_received(self, data, genre):
         header_i, header_j = 0, 1
         while header_j < data_len-4:
             if self._is_frame_header(data, header_i, header_j):
-                cmd_id = data[header_i + 3]
+                cmd_id = data[header_i + 4]
                 # compare send header and received header
                 if cmd_id == genre:
                     break
             header_i += 1
             header_j += 1
         else:
             return []
-        data_len = data[header_i + 2] - 2
+        data_len = data[header_i + 3] - 2
         unique_data = [ProtocolCode.GET_BASIC_INPUT,
                        ProtocolCode.GET_DIGITAL_INPUT]
 
         if cmd_id in unique_data:
             data_pos = header_i + 5
             data_len -= 1
         else:
-            data_pos = header_i + 4
+            data_pos = header_i + 5
         valid_data = data[data_pos: data_pos + data_len]
 
         # process valid data
@@ -291,7 +291,7 @@ def read(self):
     k = 0
     pre = 0
     t = time.time()
-    while True and time.time() - t < 0.1:
+    while True: 
         try:
             data = self._serial_port.read()
             k+=1