From 1d2d5523e5c48a53da7f8948fc3d60fa5c58e718 Mon Sep 17 00:00:00 2001 From: wxy6655 Date: Mon, 5 Sep 2022 14:21:55 +0800 Subject: [PATCH 1/3] update mybuddy api --- demo/mybuddy_encoders_test.py | 16 ++++++++++++++++ mybuddy_encoders_test.py | 25 +++++++++++++++++++++++++ pymycobot/Interface.py | 28 +++++++++++++++++++++------- pymycobot/__init__.py | 2 +- pymycobot/common.py | 2 +- 5 files changed, 64 insertions(+), 9 deletions(-) create mode 100644 demo/mybuddy_encoders_test.py create mode 100644 mybuddy_encoders_test.py diff --git a/demo/mybuddy_encoders_test.py b/demo/mybuddy_encoders_test.py new file mode 100644 index 0000000..78d47df --- /dev/null +++ b/demo/mybuddy_encoders_test.py @@ -0,0 +1,16 @@ +import time +import os +import sys +import serial +import serial.tools.list_ports + +from pymycobot import MyBuddy + + +port: str +mc: MyBuddy +port = 'COM21' +baud = 115200 +DEBUG = False +mc = MyBuddy(port, baud, debug=DEBUG) +mc.get_encoders(0) \ No newline at end of file diff --git a/mybuddy_encoders_test.py b/mybuddy_encoders_test.py new file mode 100644 index 0000000..721459b --- /dev/null +++ b/mybuddy_encoders_test.py @@ -0,0 +1,25 @@ +import time +import os +import sys +import serial +import serial.tools.list_ports + +from pymycobot.mybuddy import MyBuddy + + +port: str +mc: MyBuddy +port = 'COM21' +baud = 115200 +DEBUG = False + +encoders = [2047,2047,2047,2047,2047,2047,2047,2047,2047,2047,2047,2047,2047] +speeds = [500,500,500,500,500,500,500,500,500,500,500,500,500] + +mc = MyBuddy(port, baud, debug=DEBUG) +# print(mc.get_angles(1)) +print(mc.get_encoders(0)) + +# mc.set_encoders(0,encoders,speeds) + +# mc.set_encoders(1,[1,1,1,1,1,1],[0,2,0,2,0,2]) \ No newline at end of file diff --git a/pymycobot/Interface.py b/pymycobot/Interface.py index 23492d8..09905a9 100644 --- a/pymycobot/Interface.py +++ b/pymycobot/Interface.py @@ -38,8 +38,9 @@ def _mesg(self, genre, *args, **kwargs): command_data[1:], check_digit, ] - # print(command) + # print("write_data: ",command) real_command = self._flatten(command) + # print("write_data: ",real_command) has_reply = kwargs.get("has_reply", False) return real_command, has_reply @@ -454,17 +455,30 @@ def set_encoders(self, id, encoders, speed): """Set the six joints of the manipulator to execute synchronously to the specified position. Args: - id: 1/2 (L/R). - encoders: A encoder list, length 6. - speed: speed 1 ~ 100 - """ - return self._mesg(ProtocolCode.SET_ENCODERS, id, encoders, speed) + id: 0/1/2 (all/L/R). + if id = 0: + encoders: A encoder list, length 13. + speed: A encoder list,length 13. + else: + encoders: A encoder list, length 6. + speed: A encoder list, length 6. + """ + _id = id + _encoders = encoders + _speed = speed + encoders_data = [] + if id == 0: + encoders_data = (_encoders[0:6] + speed[0:6] + _encoders[6:12] + speed[6:12] + _encoders[-2:] + speed[-2:]) + return self._mesg(ProtocolCode.SET_ENCODERS, _id, encoders_data) + if id == 1 or id == 2: + return self._mesg(ProtocolCode.SET_ENCODERS, _id, _encoders, _speed) def get_encoders(self, id): """Get the six joints of the manipulator Args: - id: 1/2 (L/R). + id: 0/1/2 (all/L/R). + if id==0: return all joint encoder and speed, Return: The list of encoders diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py index f82c455..57192f3 100644 --- a/pymycobot/__init__.py +++ b/pymycobot/__init__.py @@ -30,7 +30,7 @@ "Mira" ] -__version__ = "2.9.3b1.333" +__version__ = "2.9.3b1.444" __author__ = "Elephantrobotics" __email__ = "weiquan.xu@elephantrobotics.com" __git_url__ = "https://github.com/elephantrobotics/pymycobot" diff --git a/pymycobot/common.py b/pymycobot/common.py index cae98a7..1d8b622 100644 --- a/pymycobot/common.py +++ b/pymycobot/common.py @@ -272,7 +272,7 @@ def _process_received(self, data, genre, arm = 6): # process valid data res = [] - if data_len in [6, 8, 12 ,24]: + if data_len in [6, 8, 12, 24, 60]: for header_i in range(0, len(valid_data), 2): one = valid_data[header_i: header_i + 2] res.append(self._decode_int16(one)) From 7a6a8d22de2e27ac5b231d637701968d214e3752 Mon Sep 17 00:00:00 2001 From: wxy6655 Date: Mon, 5 Sep 2022 18:05:39 +0800 Subject: [PATCH 2/3] update mybuddy dt --- drag_trial_teaching_mubuddy.py | 203 +++++++++++++++++++++++++++++++++ 1 file changed, 203 insertions(+) create mode 100644 drag_trial_teaching_mubuddy.py diff --git a/drag_trial_teaching_mubuddy.py b/drag_trial_teaching_mubuddy.py new file mode 100644 index 0000000..6180c1e --- /dev/null +++ b/drag_trial_teaching_mubuddy.py @@ -0,0 +1,203 @@ +''' +Drag and teach in windows version +''' +from pickle import FALSE +import time +import os +import sys +import threading +import json +import serial +import serial.tools.list_ports +import platform +import termios +import tty +from pymycobot import MyBuddy + + +mb: MyBuddy +sp: int = 80 +os_version: str +WIN = False +LINUX = False + +def setup(): + print("") + global port, mb, baud, os_version, WIN, LINUX + os_version = platform.system() + if os_version == 'Windows': + WIN = True + LINUX = False + elif os_version == 'Linux': + WIN = False + LINUX = True + if WIN: + port = 'COM21' + baud = 115200 + elif LINUX: + port = '/dev/ttyS0' + baud = 115200 + mb = MyBuddy(port, baud, debug=True) + + +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)) + print("\r{}".format(msg)) + + +class TeachingTest(Helper): + def __init__(self, mycobot) -> None: + super().__init__() + self.mb = mb + 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() + _id = 0 + while self.recording: + _encoders = self.mb.get_encoders(_id) + if _encoders: + self.record_list.append(_encoders) + time.sleep(0.05) + # 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 _encoders in self.record_list: + print(_encoders) + self.mb.set_encoders(0,_encoders) + time.sleep(0.05) + 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.mb.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 + Enter: quit + \r r + Enter: start record + \r c + Enter: stop record + \r p + Enter: play once + \r P + Enter: loop play / stop loop play + \r s + Enter: save to local + \r l + Enter: load from local + \r f + Enter: release mycobot + \r---------------------------------- + """ + ) + + def start(self): + global WIN, LINUX + self.print_menu() + while not False: + if WIN: + key = input() + elif LINUX: + 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.mb.release_all_servos(0) + time.sleep(0.05) + self.mb.release_all_servos(2) + self.echo("Released") + else: + print(key) + continue + + +if __name__ == "__main__": + setup() + recorder = TeachingTest(mb) + recorder.start() From c29760b48569e25c1383cfc1a4e084a72e7446b1 Mon Sep 17 00:00:00 2001 From: wxy6655 Date: Mon, 5 Sep 2022 18:26:35 +0800 Subject: [PATCH 3/3] update mybuddy encoders api