开始拖动试教(微处理器类设备)
适用设备
- myCobot 280-Pi
- myCobot 320-Pi
操作步骤
Atom 烧录最新版的 atomMain
在桌面新建一个 Python 文件为 *.py ,将下列代码拷贝进去并保存(在此,文件命名为:drag_trial_teaching.py)
打开一个控制终端,输入以下命令:
cd Desktop
python3 drag_trial_teaching.py
- 输入 1 ,按下回车键
- 输入相对应的波特率,按下回车键
- myCobot 280-Pi :1000000
- myCobot 320-Pi : 115200
- 是否查看 BAUD ,输入 Y / N ,按下回车键
出现功能选项
键盘输入 q ,退出此程序
键盘输入 r ,开始录制
键盘输入 c ,停止录制
键盘输入 p ,播放一次
键盘输入 P ,循环播放(如需终止循环,键盘输入 q 退出程序)
键盘输入 f ,释放机械臂各个关节(可用于机械臂停止运动,各个关节锁定时使用)
代码内容
注意: 该代码为 drag_trial_teaching.py ,可以通过 GitHub 进行下载查看。
展开查看
```python
import time
import os
import sys
import termios
import tty
import threading
import json
import serial
import serial.tools.list_ports
from pymycobot.mycobot import MyCobot
port: str
mc: MyCobot
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 = MyCobot(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 = self.mc.get_angles()
if angles:
self.record_list.append(angles)
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.send_angles(angles, 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.send_angles(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()
```