|
| 1 | +import time |
| 2 | +import os |
| 3 | +import sys |
| 4 | +import termios |
| 5 | +import tty |
| 6 | +import threading |
| 7 | +import json |
| 8 | +import serial |
| 9 | +import serial.tools.list_ports |
| 10 | + |
| 11 | +from pymycobot.mycobot import MyCobot |
| 12 | + |
| 13 | + |
| 14 | +port: str |
| 15 | +mc: MyCobot |
| 16 | +sp: int = 80 |
| 17 | + |
| 18 | + |
| 19 | +def setup(): |
| 20 | + print("") |
| 21 | + global port, mc |
| 22 | + plist = list(serial.tools.list_ports.comports()) |
| 23 | + idx = 1 |
| 24 | + for port in plist: |
| 25 | + print("{} : {}".format(idx, port)) |
| 26 | + idx += 1 |
| 27 | + |
| 28 | + _in = input("\nPlease input 1 - {} to choice:".format(idx - 1)) |
| 29 | + port = str(plist[int(_in) - 1]).split(" - ")[0].strip() |
| 30 | + print(port) |
| 31 | + print("") |
| 32 | + |
| 33 | + baud = 115200 |
| 34 | + _baud = input("Please input baud(default:115200):") |
| 35 | + try: |
| 36 | + baud = int(_baud) |
| 37 | + except Exception: |
| 38 | + pass |
| 39 | + print(baud) |
| 40 | + print("") |
| 41 | + |
| 42 | + DEBUG = False |
| 43 | + f = input("Wether DEBUG mode[Y/n]:") |
| 44 | + if f in ["y", "Y", "yes", "Yes"]: |
| 45 | + DEBUG = True |
| 46 | + # mc = MyCobot(port, debug=True) |
| 47 | + mc = MyCobot(port, baud, debug=DEBUG) |
| 48 | + |
| 49 | + |
| 50 | +class Raw(object): |
| 51 | + """Set raw input mode for device""" |
| 52 | + |
| 53 | + def __init__(self, stream): |
| 54 | + self.stream = stream |
| 55 | + self.fd = self.stream.fileno() |
| 56 | + |
| 57 | + def __enter__(self): |
| 58 | + self.original_stty = termios.tcgetattr(self.stream) |
| 59 | + tty.setcbreak(self.stream) |
| 60 | + |
| 61 | + def __exit__(self, type, value, traceback): |
| 62 | + termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty) |
| 63 | + |
| 64 | + |
| 65 | +class Helper(object): |
| 66 | + def __init__(self) -> None: |
| 67 | + self.w, self.h = os.get_terminal_size() |
| 68 | + |
| 69 | + def echo(self, msg): |
| 70 | + print("\r{}".format(" " * self.w), end="") |
| 71 | + print("\r{}".format(msg), end="") |
| 72 | + |
| 73 | + |
| 74 | +class TeachingTest(Helper): |
| 75 | + def __init__(self, mycobot) -> None: |
| 76 | + super().__init__() |
| 77 | + self.mc = mycobot |
| 78 | + self.recording = False |
| 79 | + self.playing = False |
| 80 | + self.record_list = [] |
| 81 | + self.record_t = None |
| 82 | + self.play_t = None |
| 83 | + |
| 84 | + def record(self): |
| 85 | + self.record_list = [] |
| 86 | + self.recording = True |
| 87 | + |
| 88 | + def _record(): |
| 89 | + start_t = time.time() |
| 90 | + |
| 91 | + while self.recording: |
| 92 | + angles = self.mc.get_encoders() |
| 93 | + speeds = self.mc.get_servo_speeds() |
| 94 | + if angles: |
| 95 | + self.record_list.append([angles, speeds]) |
| 96 | + time.sleep(0.1) |
| 97 | + print("\r {}".format(time.time() - start_t), end="") |
| 98 | + |
| 99 | + self.echo("Start recording.") |
| 100 | + self.record_t = threading.Thread(target=_record, daemon=True) |
| 101 | + self.record_t.start() |
| 102 | + |
| 103 | + def stop_record(self): |
| 104 | + if self.recording: |
| 105 | + self.recording = False |
| 106 | + self.record_t.join() |
| 107 | + self.echo("Stop record") |
| 108 | + |
| 109 | + def play(self): |
| 110 | + self.echo("Start play") |
| 111 | + for angles in self.record_list: |
| 112 | + # print(angles) |
| 113 | + self.mc.send_encoders_drag(angles[0], angles[1]) |
| 114 | + time.sleep(0.1) |
| 115 | + self.echo("Finish play") |
| 116 | + |
| 117 | + def loop_play(self): |
| 118 | + self.playing = True |
| 119 | + |
| 120 | + def _loop(): |
| 121 | + len_ = len(self.record_list) |
| 122 | + i = 0 |
| 123 | + while self.playing: |
| 124 | + idx_ = i % len_ |
| 125 | + i += 1 |
| 126 | + self.mc.send_encoders_drag(self.record_list[idx_][0], self.record_list[idx_][1]) |
| 127 | + time.sleep(0.1) |
| 128 | + |
| 129 | + self.echo("Start loop play.") |
| 130 | + self.play_t = threading.Thread(target=_loop, daemon=True) |
| 131 | + self.play_t.start() |
| 132 | + |
| 133 | + def stop_loop_play(self): |
| 134 | + if self.playing: |
| 135 | + self.playing = False |
| 136 | + self.play_t.join() |
| 137 | + self.echo("Stop loop play.") |
| 138 | + |
| 139 | + def save_to_local(self): |
| 140 | + if not self.record_list: |
| 141 | + self.echo("No data should save.") |
| 142 | + return |
| 143 | + |
| 144 | + with open(os.path.dirname(__file__) + "/record.txt", "w") as f: |
| 145 | + json.dump(self.record_list, f, indent=2) |
| 146 | + self.echo("save dir: {}".format(os.path.dirname(__file__))) |
| 147 | + |
| 148 | + def load_from_local(self): |
| 149 | + |
| 150 | + with open(os.path.dirname(__file__) + "/record.txt", "r") as f: |
| 151 | + try: |
| 152 | + data = json.load(f) |
| 153 | + self.record_list = data |
| 154 | + self.echo("Load data success.") |
| 155 | + except Exception: |
| 156 | + self.echo("Error: invalid data.") |
| 157 | + |
| 158 | + def print_menu(self): |
| 159 | + print( |
| 160 | + """\ |
| 161 | + \r q: quit |
| 162 | + \r r: start record |
| 163 | + \r c: stop record |
| 164 | + \r p: play once |
| 165 | + \r P: loop play / stop loop play |
| 166 | + \r s: save to local |
| 167 | + \r l: load from local |
| 168 | + \r f: release mycobot |
| 169 | + \r---------------------------------- |
| 170 | + """ |
| 171 | + ) |
| 172 | + |
| 173 | + def start(self): |
| 174 | + self.print_menu() |
| 175 | + |
| 176 | + while not False: |
| 177 | + with Raw(sys.stdin): |
| 178 | + key = sys.stdin.read(1) |
| 179 | + if key == "q": |
| 180 | + break |
| 181 | + elif key == "r": # recorder |
| 182 | + self.record() |
| 183 | + elif key == "c": # stop recorder |
| 184 | + self.stop_record() |
| 185 | + elif key == "p": # play |
| 186 | + self.play() |
| 187 | + elif key == "P": # loop play |
| 188 | + if not self.playing: |
| 189 | + self.loop_play() |
| 190 | + else: |
| 191 | + self.stop_loop_play() |
| 192 | + elif key == "s": # save to local |
| 193 | + self.save_to_local() |
| 194 | + elif key == "l": # load from local |
| 195 | + self.load_from_local() |
| 196 | + elif key == "f": # free move |
| 197 | + self.mc.release_all_servos() |
| 198 | + self.echo("Released") |
| 199 | + else: |
| 200 | + print(key) |
| 201 | + continue |
| 202 | + |
| 203 | + |
| 204 | +if __name__ == "__main__": |
| 205 | + setup() |
| 206 | + recorder = TeachingTest(mc) |
| 207 | + recorder.start() |
0 commit comments