实现拖动示教
1 适用设备
- myCobot 280 Pi
- myCobot 320 Pi
- mechArm 270 Pi
2 操作步骤
Step 1: Atom烧录最新版的atomMain。
Step 2: 在桌面新建一个 Python 文件为 *.py ，将下列代码拷贝进去并保存。
注意：文件命名为：drag_trial_teaching.py。
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()
Step 3: 打开终端（快捷键CTRL+ALT+t），输入以下命令：
cd Desktop
python3 drag_trial_teaching.py
Step 4: 点击回车，会出现指令Please input 1 - 1 to choice:，输入1，回车。
Step 5: 输入相对应的波特率，按下回车键。
- myCobot 280-Pi：1000000
- myCobot 320-Pi：115200
Step 6: 是否查看BAUD，输入Y/N，按下回车键。
Step 7: 此时可以开始拖拽机械臂。
- 键盘输入q，退出此程序
- 键盘输入 r ，开始录制
这里输入了r
Step 8: 键盘输入c，停止录制
Step 9: 键盘输入p，播放一次。
输入P，循环播放
输入q终止循环，退出程序
输入f，释放机械臂各个关节（可用于机械臂停止运动，各个关节锁定时使用）