├── pykeigan ├── __init__.py ├── utils.py ├── blecontroller.py ├── usbcontroller.py └── controller.py ├── devel-requirements.txt ├── tests ├── __init__.py └── test_units.py ├── Makefile ├── requirements-usb.txt ├── requirements.txt ├── .travis.yml ├── examples ├── KM1Scan.py ├── windows_examples │ ├── run.py │ ├── reset_all.py │ ├── select_port_run.py │ ├── taskset_run.py │ ├── baudrate_change.py │ ├── run_continious.py │ ├── baudrate_run.py │ ├── taskset_move.py │ ├── keigan_motor_initial_setting.py │ ├── move_wait_queue.py │ ├── pos_arrival_test.py │ ├── move_to_continious.py │ ├── sine_wave_move.py │ ├── motor_measurement.py │ ├── pid_settings.py │ └── keigan_motor_measurement_test.py ├── ble-rotate-the-motor.py ├── ble-simple-connection.py ├── usb-rotate-the-motor.py ├── usb-button_trigger.py ├── usb-simple-connection.py ├── ble-scanner-connection.py ├── ble-get-motor-Informations.py ├── readme.md ├── usb-move_wait_queue.py ├── usb-get-motor-Info-time.py ├── usb-actuator.py ├── usb-get-motor-Informations.py ├── usb-sine-wave.py ├── usb-torque-control.py ├── usb-position-control.py └── usb-teaching-control.py ├── tools ├── KM1Scan.py └── log │ ├── sample.py │ ├── readme.md │ └── log.py ├── MIT-LICENSE ├── setup.py ├── .gitignore └── README.rst /pykeigan/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /devel-requirements.txt: -------------------------------------------------------------------------------- 1 | pytest >= 5.0.1 2 | pytest-cov >= 2.7.1 3 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append("../pykeigan") 3 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | init: 2 | pip install -r requirements.txt 3 | 4 | test: 5 | nosetests tests 6 | -------------------------------------------------------------------------------- /requirements-usb.txt: -------------------------------------------------------------------------------- 1 | # 2 | # This file is autogenerated by pip-compile 3 | # To update, run: 4 | # 5 | # pip-compile --output-file requirements.txt requirements.in 6 | # 7 | pyserial>=3.4 8 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # 2 | # This file is autogenerated by pip-compile 3 | # To update, run: 4 | # 5 | # pip-compile --output-file requirements.txt requirements.in 6 | # 7 | bluepy>=1.1.4 8 | pyserial>=3.4 9 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: xenial # required for Python >= 3.7 2 | language: python 3 | python: 4 | - "3.7" 5 | install: 6 | - pip install -r requirements.txt -r devel-requirements.txt 7 | script: 8 | - pytest -v --cov=pykeigan --cov-report=html 9 | -------------------------------------------------------------------------------- /examples/KM1Scan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Jan 14 10:46:12 2018 5 | 6 | @author: takata@innovotion.co.jp 7 | """ 8 | from __future__ import print_function 9 | from bluepy.btle import Scanner 10 | import sys 11 | if len(sys.argv)>=2: 12 | scan_sec=float(sys.argv[1]) 13 | else: 14 | scan_sec=5.0 15 | scanner=Scanner() 16 | devices=scanner.scan(scan_sec) 17 | KM1_list=[] 18 | for dev in devices: 19 | for (adtype, desc, value) in dev.getScanData(): 20 | if desc=="Complete Local Name" and "KM-1" in value: 21 | print(value,":",dev.addr) 22 | -------------------------------------------------------------------------------- /tools/KM1Scan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Jan 14 10:46:12 2018 5 | 6 | @author: takata@innovotion.co.jp 7 | """ 8 | from __future__ import print_function 9 | from bluepy.btle import Scanner 10 | import sys 11 | if len(sys.argv)>=2: 12 | scan_sec=float(sys.argv[1]) 13 | else: 14 | scan_sec=5.0 15 | scanner=Scanner() 16 | devices=scanner.scan(scan_sec) 17 | KM1_list=[] 18 | for dev in devices: 19 | for (adtype, desc, value) in dev.getScanData(): 20 | if desc=="Complete Local Name" and "KM-1" in value: 21 | print(value,":",dev.addr) 22 | -------------------------------------------------------------------------------- /tests/test_units.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | from pykeigan.utils import * 3 | 4 | def test_float2bytes(): 5 | result = float2bytes(1); 6 | assert result == b'?\x80\x00\00' 7 | 8 | def test_bytes2float(): 9 | result = bytes2float(b'?\x80\x00\00'); 10 | assert result == 1 11 | 12 | def test_uint8_t2bytes(self): 13 | assert uint8_t2bytes(23)==b'\x17' 14 | 15 | def test_bytes2uint8_t(self): 16 | assert bytes2uint8_t(b'\x17')==23 17 | 18 | def test_uint16_t2bytes(self): 19 | assert uint16_t2bytes(23)==b'\x00\x17' 20 | 21 | def test_bytes2uint16_t(self): 22 | assert bytes2int16_t(b'\x00\x17')==23 23 | 24 | def test_bytes2int16_t(self): 25 | assert bytes2int16_t(b'\xff\xff')==-1 26 | 27 | def test_uint32_t2bytes(self): 28 | assert uint32_t2bytes(16**6)==b'\x01\x00\x00\x00' 29 | -------------------------------------------------------------------------------- /MIT-LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2017 Innovotion Inc. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from setuptools import setup, find_packages 4 | from os import path 5 | from io import open 6 | 7 | import codecs 8 | this_directory = path.abspath(path.dirname(__file__)) 9 | readme = open(path.join(this_directory, 'README.rst'), encoding='utf-8').read().replace("\r", "") 10 | 11 | # This will be an error by Windows: `long_description` has syntax errors in markup and would not be rendered on PyPI. 12 | # with codecs.open(path.join(this_directory, 'README.rst'),'r','utf-8') as f: 13 | # readme = f.read() 14 | 15 | setup( 16 | name='pykeigan_motor', 17 | version='2.4.0', 18 | description='Python Library for Keigan Motors (v2)', 19 | long_description=readme, 20 | long_description_content_type='text/x-rst', 21 | install_requires=['pyserial'], 22 | extras_require={'ble' : ['bluepy']}, 23 | author='Tomohiro Takata, Hiroshi Harada, Takashi Tokuda', 24 | author_email='takata@innovotion.co.jp, harada@keigan.co.jp, tokuda@keigan.co.jp', 25 | url='https://github.com/keigan-motor/pykeigan_motor', 26 | license='MIT-LICENSE', 27 | packages=find_packages(exclude=('tests', 'docs')) 28 | ) 29 | -------------------------------------------------------------------------------- /examples/windows_examples/run.py: -------------------------------------------------------------------------------- 1 | import msvcrt 2 | from time import sleep 3 | import argparse 4 | import sys 5 | import pathlib 6 | 7 | current_dir = pathlib.Path(__file__).resolve().parent 8 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 9 | 10 | from pykeigan import usbcontroller 11 | from pykeigan import utils 12 | 13 | # Select port connecting to KeiganMotor 14 | port = 'COM7' 15 | dev = usbcontroller.USBController(port) 16 | 17 | 18 | if __name__ == '__main__': 19 | try: 20 | while True: 21 | sleep(0.01) 22 | if msvcrt.kbhit(): 23 | c = msvcrt.getwch() 24 | print(c) 25 | 26 | if c == 'r': 27 | # rpm -> radian/sec 28 | dev.enable_action() 29 | dev.set_speed(utils.rpm2rad_per_sec(5)) 30 | dev.set_led(1, 0, 200, 0) 31 | dev.run_forward() 32 | elif c == 's': 33 | dev.stop_motor() 34 | 35 | except KeyboardInterrupt: 36 | if dev: 37 | dev.disable_action() 38 | print('Ctrl-C') 39 | -------------------------------------------------------------------------------- /examples/ble-rotate-the-motor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | import sys 10 | import pathlib 11 | 12 | current_dir = pathlib.Path(__file__).resolve().parent 13 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 14 | 15 | from time import sleep 16 | from pykeigan import blecontroller 17 | from pykeigan import utils 18 | 19 | """ 20 | ---------------------- 21 | モーターを5rpmで 正転(10秒) -> 逆転(10秒) -> 停止(トルクあり) -> 停止(トルク無し) 22 | ---------------------- 23 | 24 | """ 25 | 26 | dev=blecontroller.BLEController("d1:5a:fa:a7:d9:5d")#モーターのMACアドレス 参照 ble-simple-connection.py 27 | dev.enable_action()#安全装置。初めてモーターを動作させる場合に必ず必要。 28 | dev.set_speed(utils.rpm2rad_per_sec(5))#rpm -> radian/sec 29 | 30 | dev.set_led(1, 0, 200, 0) 31 | dev.run_forward() 32 | 33 | sleep(10) 34 | 35 | dev.set_led(1, 0, 0, 200) 36 | dev.run_reverse() 37 | 38 | sleep(10) 39 | 40 | dev.set_led(1, 200, 0, 0) 41 | dev.stop_motor() 42 | 43 | sleep(10) 44 | 45 | dev.set_led(1, 100, 100, 100) 46 | dev.free_motor() 47 | 48 | 49 | """ 50 | Exit with key input 51 | """ 52 | while True: 53 | print("---------------------------------------") 54 | if sys.version_info<(3,0): 55 | inp = raw_input('Exit:[key input] >>') 56 | else: 57 | inp = input('Exit:[key input] >>') 58 | if inp !=None: 59 | dev.set_led(1, 100, 100, 100) 60 | dev.disconnect() 61 | break -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | 27 | # PyInstaller 28 | # Usually these files are written by a python script from a template 29 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 30 | *.manifest 31 | *.spec 32 | 33 | # Installer logs 34 | pip-log.txt 35 | pip-delete-this-directory.txt 36 | 37 | # Unit test / coverage reports 38 | htmlcov/ 39 | .tox/ 40 | .coverage 41 | .coverage.* 42 | .cache 43 | nosetests.xml 44 | coverage.xml 45 | *,cover 46 | .hypothesis/ 47 | 48 | # Translations 49 | *.mo 50 | *.pot 51 | 52 | # Django stuff: 53 | *.log 54 | local_settings.py 55 | 56 | # Flask stuff: 57 | instance/ 58 | .webassets-cache 59 | 60 | # Scrapy stuff: 61 | .scrapy 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | # IPython Notebook 70 | .ipynb_checkpoints 71 | 72 | # pyenv 73 | .python-version 74 | 75 | # celery beat schedule file 76 | celerybeat-schedule 77 | 78 | # dotenv 79 | .env 80 | 81 | # virtualenv 82 | .venv/ 83 | venv/ 84 | ENV/ 85 | 86 | # Spyder project settings 87 | .spyderproject 88 | 89 | # Rope project settings 90 | .ropeproject 91 | 92 | # PyCharm IDE 93 | .idea/ 94 | 95 | # test 96 | .pytest_cache 97 | .vscode/ 98 | -------------------------------------------------------------------------------- /examples/ble-simple-connection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | import sys 10 | import pathlib 11 | 12 | current_dir = pathlib.Path(__file__).resolve().parent 13 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 14 | 15 | from pykeigan import blecontroller 16 | from pykeigan import utils 17 | 18 | """ 19 | ---------------------- 20 | モーターへの接続 21 | ---------------------- 22 | モーターのMACアドレスについて 23 | MACアドレスは"nRF Connect"を使用して調べる事が可能です。 24 | https://play.google.com/store/apps/details?id=no.nordicsemi.android.mcp 25 | 26 | Bluetoohでモーターが検出されない場合は、モーターの"インターフェイスのリセット"を実行して下さい。 27 | https://document.keigan-motor.com/basic/reset 28 | """ 29 | 30 | dev=blecontroller.BLEController("d1:5a:fa:a7:d9:5d")#モーターのMACアドレス 31 | dev.set_led(2,255,255,0)# (LEDflash:2,R:255,G:255,B:0) https://document.keigan-motor.com/software_dev/lowapis/led 32 | dev.enable_action()#安全装置。初めてモーターを動作させる場合に必ず必要。 33 | dev.set_speed(utils.rpm2rad_per_sec(5))#rpm -> radian/sec 34 | dev.run_forward() 35 | 36 | """ 37 | Exit with key input 38 | """ 39 | while True: 40 | print("---------------------------------------") 41 | if sys.version_info<(3,0): 42 | inp = raw_input('Exit:[key input] >>') 43 | else: 44 | inp = input('Exit:[key input] >>') 45 | if inp !=None: 46 | dev.set_led(1, 100, 100, 100) 47 | dev.disable_action() 48 | dev.disconnect() 49 | break -------------------------------------------------------------------------------- /examples/windows_examples/reset_all.py: -------------------------------------------------------------------------------- 1 | import msvcrt 2 | from time import sleep 3 | import argparse 4 | import sys 5 | import pathlib 6 | 7 | import serial 8 | import serial.tools.list_ports 9 | 10 | current_dir = pathlib.Path(__file__).resolve().parent 11 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 12 | 13 | from pykeigan import usbcontroller 14 | from pykeigan import utils 15 | 16 | 17 | def select_port(): 18 | print('Available COM ports list') 19 | 20 | portlist = serial.tools.list_ports.comports() 21 | 22 | if not portlist: 23 | print('No available port') 24 | sys.exit() 25 | 26 | print('i : name') 27 | print('--------') 28 | for i, port in enumerate(portlist): 29 | print(i, ':', port.device) 30 | 31 | print('- Enter the port number (0~)') 32 | portnum = input() 33 | portnum = int(portnum) 34 | 35 | portdev = None 36 | if portnum in range(len(portlist)): 37 | portdev = portlist[portnum].device 38 | 39 | print('Conncted to', portdev) 40 | 41 | return portdev 42 | 43 | # Select port connecting to KeiganMotor 44 | port = select_port() 45 | dev = usbcontroller.USBController(port) 46 | 47 | 48 | if __name__ == '__main__': 49 | try: 50 | while True: 51 | sleep(0.01) 52 | if msvcrt.kbhit(): 53 | c = msvcrt.getwch() 54 | print(c) 55 | 56 | if c == 'r': 57 | # rpm -> radian/sec 58 | dev.reset_all_registers() 59 | dev.save_all_registers() 60 | 61 | except KeyboardInterrupt: 62 | if dev: 63 | dev.disable_action() 64 | print('Ctrl-C') 65 | -------------------------------------------------------------------------------- /examples/usb-rotate-the-motor.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | from time import sleep 13 | 14 | current_dir = pathlib.Path(__file__).resolve().parent 15 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 16 | 17 | from pykeigan import usbcontroller 18 | from pykeigan import utils 19 | 20 | parser = argparse.ArgumentParser(description='モーター動作 正転 逆転') 21 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 22 | args = parser.parse_args() 23 | 24 | """ 25 | ---------------------- 26 | モーターを5rpmで 正転(10秒) -> 逆転(10秒) -> 停止(トルクあり) -> 停止(トルク無し) 27 | ---------------------- 28 | 29 | """ 30 | dev=usbcontroller.USBController(args.port,False)#モーターのアドレス 参照 usb-simple-connection.py 31 | dev.enable_action()#安全装置。初めてモーターを動作させる場合に必ず必要。 32 | dev.set_speed(utils.rpm2rad_per_sec(5))#rpm -> radian/sec 33 | 34 | dev.set_led(1, 0, 200, 0) 35 | dev.run_forward() 36 | 37 | sleep(10) 38 | 39 | dev.set_led(1, 0, 0, 200) 40 | dev.run_reverse() 41 | 42 | sleep(10) 43 | 44 | dev.set_led(1, 200, 0, 0) 45 | dev.stop_motor() 46 | 47 | sleep(10) 48 | 49 | dev.set_led(1, 100, 100, 100) 50 | dev.free_motor() 51 | 52 | 53 | """ 54 | Exit with key input 55 | """ 56 | while True: 57 | print("---------------------------------------") 58 | if sys.version_info <(3,0): 59 | inp = raw_input('Exit:[Other key] >>') 60 | else: 61 | inp = input('Exit:[Other key] >>') 62 | if inp !=None: 63 | dev.set_led(1, 100, 100, 100) 64 | dev.disconnect() 65 | break 66 | -------------------------------------------------------------------------------- /examples/usb-button_trigger.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Jan 23 2020 4 | @author: tokuda@keigan.co.jp 5 | """ 6 | 7 | import sys 8 | import pathlib 9 | 10 | from time import sleep 11 | 12 | current_dir = pathlib.Path(__file__).resolve().parent 13 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 14 | 15 | from pykeigan import usbcontroller 16 | from pykeigan import utils 17 | 18 | port1='/dev/ttyUSB0' 19 | 20 | """ 21 | ---------------------- 22 | モーター側のイベントを受信したときに呼ばれるコールバック 23 | ---------------------- 24 | event_type: 'button' はボタン押下を意味する 25 | その場合、number: は押したボタンの番号（左から1/2/3） 26 | """ 27 | def on_motor_event_cb(event): 28 | print("\033[3;2H\033[2K") 29 | print('event {} '.format(event)) 30 | sys.stdout.flush() 31 | if event['event_type'] == 'button': 32 | print('button!!') 33 | if event['number'] == 2: 34 | print('2!!') 35 | motor1.disable_action() 36 | elif event['number'] == 3: 37 | print('3!!') 38 | motor1.enable_action() 39 | motor1.run_at_velocity(utils.rpm2rad_per_sec(5)) 40 | else: 41 | print(event['number']) 42 | 43 | 44 | 45 | print("---------------------------------------") 46 | print("Button Trigger Test Start!") 47 | 48 | # KeiganMotor のデバイスアドレス（ポート）を指定し、初期化を行う 49 | motor1=usbcontroller.USBController(port1,False) 50 | motor1.on_motor_event_cb = on_motor_event_cb 51 | motor1.set_button_setting(30) # Set buttons as Parent mode 52 | motor1.enable_action() 53 | motor1.run_at_velocity(utils.rpm2rad_per_sec(5)) 54 | 55 | 56 | """ 57 | Exit with key input 58 | """ 59 | try: 60 | while True: 61 | sleep(0.1) 62 | 63 | except KeyboardInterrupt: 64 | motor1.set_led(1, 255, 0, 0) 65 | motor1.disconnect() 66 | -------------------------------------------------------------------------------- /examples/usb-simple-connection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | import argparse 10 | from argparse import RawTextHelpFormatter 11 | import sys 12 | import pathlib 13 | import time 14 | 15 | current_dir = pathlib.Path(__file__).resolve().parent 16 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 17 | 18 | from pykeigan import usbcontroller 19 | from pykeigan import utils 20 | 21 | description=""" 22 | ---------------------- 23 | モーターへの接続 24 | ---------------------- 25 | モーターのデバイスファイル指定について 26 | "/dev/ttyUSB0"で表示されるデバイス名での接続は、複数のモーターを接続した場合に変わる可能性がある。 27 | 複数のモーターがある場合で、モーターを特定して接続する場合は "$ls /dev/serial/by-id/" で表示されるデバイスを使用する。 28 | ex)/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DM00LSSA-if00-port0 29 | 30 | """ 31 | parser = argparse.ArgumentParser(description=description,formatter_class=RawTextHelpFormatter) 32 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 33 | args = parser.parse_args() 34 | 35 | dev=usbcontroller.USBController(args.port,False) 36 | dev.set_led(2,255,255,0)# (LEDflash:2,R:255,G:255,B:0) https://document.keigan-motor.com/software_dev/lowapis/led 37 | dev.enable_action()#安全装置。初めてモーターを動作させる場合に必ず必要。 38 | dev.set_speed(utils.rpm2rad_per_sec(5))#rpm -> radian/sec 39 | dev.run_forward() 40 | 41 | """ 42 | Exit with key input 43 | """ 44 | while True: 45 | print("---------------------------------------") 46 | if sys.version_info < (3, 0): inp = raw_input('Exit:[key input] >>') 47 | else: inp = input('Exit:[key input] >>') 48 | if inp !=None: 49 | dev.set_led(1, 100, 100, 100) 50 | time.sleep(1) 51 | dev.disable_action() #USB接続は切断してもモーター動作は停止しない為、明示的に停止する必要がある 52 | dev.disconnect() 53 | break 54 | -------------------------------------------------------------------------------- /examples/windows_examples/select_port_run.py: -------------------------------------------------------------------------------- 1 | import msvcrt 2 | import serial 3 | import serial.tools.list_ports 4 | import sys 5 | import pathlib 6 | from time import sleep 7 | 8 | current_dir = pathlib.Path(__file__).resolve().parent 9 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 10 | 11 | from pykeigan import usbcontroller 12 | from pykeigan import utils 13 | 14 | 15 | def select_port(): 16 | print('Available COM ports list') 17 | 18 | portlist = serial.tools.list_ports.comports() 19 | 20 | if not portlist: 21 | print('No available port') 22 | sys.exit() 23 | 24 | print('i : name') 25 | print('--------') 26 | for i, port in enumerate(portlist): 27 | print(i, ':', port.device) 28 | 29 | print('- Enter the port number (0~)') 30 | portnum = input() 31 | portnum = int(portnum) 32 | 33 | portdev = None 34 | if portnum in range(len(portlist)): 35 | portdev = portlist[portnum].device 36 | 37 | print('Conncted to', portdev) 38 | 39 | return portdev 40 | 41 | 42 | if __name__ == '__main__': 43 | port = select_port() 44 | # with usbcontroller.USBController(port) as dev: 45 | dev = usbcontroller.USBController(port) 46 | try: 47 | while True: 48 | sleep(0.01) 49 | if msvcrt.kbhit(): 50 | c = msvcrt.getwch() 51 | print(c) 52 | 53 | if c == 'r': 54 | # rpm -> radian/sec 55 | dev.enable_action() 56 | dev.set_speed(utils.rpm2rad_per_sec(50)) 57 | dev.set_led(1, 0, 200, 0) 58 | dev.run_forward() 59 | elif c == 's': 60 | dev.stop_motor() 61 | elif c == 'a': 62 | dev.reboot() 63 | 64 | except KeyboardInterrupt: 65 | if dev: 66 | dev.disable_action() 67 | print('Ctrl-C') 68 | -------------------------------------------------------------------------------- /examples/ble-scanner-connection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | import sys 10 | import pathlib 11 | 12 | current_dir = pathlib.Path(__file__).resolve().parent 13 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 14 | 15 | from pykeigan import blecontroller 16 | from bluepy.btle import Scanner 17 | 18 | scan_sec=10.0 19 | target_dev=None 20 | 21 | """ 22 | ---------------------- 23 | モーターを検出する 24 | ---------------------- 25 | raspberryPi等のOSではBluetoohのScanner実行時にルート権限が必要な為、sudoで実行するか以下の権限を付加する必要があります 26 | ex) sudo setcap 'cap_net_raw,cap_net_admin+eip' bluepy-helper 27 | 28 | Bluetoohでモーターが検出されない場合は、モーターの"インターフェイスのリセット"を実行して下さい。 29 | https://document.keigan-motor.com/basic/reset 30 | """ 31 | def scan(scan_sec): 32 | print("During scanning at ", scan_sec, "sec") 33 | scanner = Scanner() 34 | devices = scanner.scan(scan_sec) 35 | 36 | for dev in devices: 37 | for (adtype, desc, value) in dev.getScanData(): 38 | if desc == "Complete Local Name" and "KM-1" in value: 39 | print(value, ":", dev.addr) 40 | connection(dev,dev.addr) 41 | break 42 | 43 | """ 44 | ---------------------- 45 | モーターに接続し、LEDを黄色で点滅させる 46 | ---------------------- 47 | """ 48 | def connection(dev,macdress): 49 | target_dev = blecontroller.BLEController(macdress) 50 | target_dev.set_led(2,255,255,0)# (LEDflash:2,R:255,G:255,B:0)　https://document.keigan-motor.com/software_dev/lowapis/led 51 | 52 | 53 | """ 54 | Exit with key input 55 | """ 56 | while True: 57 | print("---------------------------------------") 58 | if sys.version_info<(3,0): 59 | inp = raw_input('Rescan:[s] Exit:[Other key] >>') 60 | else: 61 | inp = input('Rescan:[s] Exit:[Other key] >>') 62 | if inp == 's': 63 | scan(10) 64 | elif inp !=None: 65 | if target_dev: 66 | target_dev.set_led(1, 100, 100, 100) 67 | target_dev.disconnect() 68 | break -------------------------------------------------------------------------------- /examples/windows_examples/taskset_run.py: -------------------------------------------------------------------------------- 1 | import msvcrt 2 | from time import sleep 3 | 4 | current_dir = pathlib.Path(__file__).resolve().parent 5 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 6 | 7 | from pykeigan import usbcontroller 8 | from pykeigan import utils 9 | 10 | # How to use (this sample was made for Windows) 11 | # (this sample was made for Windows) 12 | # 13 | # (1) Define port 'COM?' that connected to KeiganMotor (You can see by DeviceManager) 14 | # (2) Set index to record and do taskset (selectable from 0 to 49) 15 | # (3) Set repeat to repeat when doing taskset (0~, 0 will repeat infinitely) 16 | # (4) Excute this file 17 | # (5) Press 'r' to record my_taskset in KeiganMotor 18 | # (6) Press 'd' to do taskset 19 | 20 | # Select port connecting to KeiganMotor 21 | port = 'COM7' 22 | dev = usbcontroller.USBController(port) 23 | 24 | index = 0 # index to record and do taskset 25 | repeat = 1 # repeating number to do taskset 26 | 27 | # Edit this function to record your taskset 28 | # (Recorded in KeiganMotor) 29 | def my_taskset(): 30 | dev.set_speed(utils.rpm2rad_per_sec(5)) 31 | dev.set_led(1, 0, 200, 0) 32 | dev.run_forward() 33 | dev.wait_queue(5000) # wait for 5000 ms 34 | dev.stop_motor() 35 | 36 | def record_taskset(): 37 | dev.erase_taskset(index) 38 | sleep(0.5) 39 | dev.start_recording_taskset(index) 40 | my_taskset() 41 | dev.stop_recording_taskset() 42 | 43 | def do_taskset(): 44 | dev.enable_action() 45 | dev.start_doing_taskset(index, repeat) 46 | 47 | 48 | if __name__ == '__main__': 49 | try: 50 | while True: 51 | sleep(0.01) 52 | if msvcrt.kbhit(): 53 | c = msvcrt.getwch() 54 | # print(c) 55 | 56 | if c == 'r': 57 | print('Record taskset index: %d' % index) 58 | record_taskset() 59 | 60 | elif c == 'd': 61 | print('Do taskset index: %d, repeat: %d', index, repeat) 62 | do_taskset() 63 | 64 | except KeyboardInterrupt: 65 | if dev: 66 | dev.disable_action() 67 | print('Ctrl-C') 68 | -------------------------------------------------------------------------------- /examples/windows_examples/baudrate_change.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import serial 13 | import msvcrt 14 | import serial.tools.list_ports 15 | from time import sleep 16 | 17 | current_dir = pathlib.Path(__file__).resolve().parent 18 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 19 | 20 | from pykeigan import usbcontroller 21 | from pykeigan import utils 22 | 23 | """ 24 | ---------------------- 25 | Change Baud Rate 26 | You can check new baud rate by "baudrate_run.py" after executing this sample 27 | Set the current baud rate to current_baud 28 | ボーレートの変更 29 | 本サンプル実行後、"baudrate_run.py" で動作確認が可能 30 | 以下の current_baud に現在のボーレートを入れること 31 | ---------------------- 32 | 33 | """ 34 | current_baud = 115200 35 | 36 | def select_port(): 37 | print('Available COM ports list') 38 | 39 | portlist = serial.tools.list_ports.comports() 40 | 41 | if not portlist: 42 | print('No available port') 43 | sys.exit() 44 | 45 | print('i : name') 46 | print('--------') 47 | for i, port in enumerate(portlist): 48 | print(i, ':', port.device) 49 | 50 | print('- Enter the port number (0~)') 51 | portnum = input() 52 | portnum = int(portnum) 53 | 54 | portdev = None 55 | if portnum in range(len(portlist)): 56 | portdev = portlist[portnum].device 57 | 58 | print('Conncted to', portdev) 59 | 60 | return portdev 61 | 62 | def baud_rate_setting(): 63 | print('Select baud rate to set') 64 | print('--------') 65 | print('0: 115200') 66 | print('1: 230400') 67 | print('2: 250000') 68 | print('3: 460800') 69 | print('4: 921600') 70 | print('5: 1000000 (1M)') 71 | print('--------') 72 | num = int(input()) 73 | while num < 0 or num > 5: 74 | print('Invalid value!') 75 | num = int(input()) 76 | return num 77 | 78 | 79 | dev=usbcontroller.USBController(select_port(),baud=current_baud) # Set the current baudrate to communicate 80 | dev.set_baud_rate(baud_rate_setting()) #5: 1Mbps 81 | dev.save_all_registers() 82 | sleep(1) 83 | dev.reboot() 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /examples/windows_examples/run_continious.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import utils 20 | from pykeigan import usbcontroller 21 | 22 | def select_port(): 23 | print('Available COM ports list') 24 | 25 | portlist = serial.tools.list_ports.comports() 26 | 27 | if not portlist: 28 | print('No available port') 29 | sys.exit() 30 | 31 | print('i : name') 32 | print('--------') 33 | for i, port in enumerate(portlist): 34 | print(i, ':', port.device) 35 | 36 | print('- Enter the port number (0~)') 37 | portnum = input() 38 | portnum = int(portnum) 39 | 40 | portdev = None 41 | if portnum in range(len(portlist)): 42 | portdev = portlist[portnum].device 43 | 44 | print('Connected to', portdev) 45 | 46 | return portdev 47 | 48 | 49 | 50 | # ログ情報callback 51 | def on_motor_log_cb(log): 52 | print('log {} '.format(log)) 53 | 54 | if log['error_codes'] == 'KM_SUCCESS': 55 | print('Command Success') 56 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 57 | print('Position Arrival Success') 58 | 59 | 60 | 61 | 62 | # モーター回転情報callback 63 | def on_motor_measurement_cb(measurement): 64 | print("\r"+'measurement {} '.format(measurement), end="") 65 | 66 | 67 | dev = usbcontroller.USBController(select_port()) 68 | dev.on_motor_log_cb = on_motor_log_cb 69 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 70 | dev.disable_action() 71 | dev.enable_action() 72 | dev.set_curve_type(0) 73 | dev.set_speed_p(14) 74 | dev.set_safe_run_settings(True, 1000, 1) # 第1引数が True の場合、5000[ms]以内に次の動作命令が来ないと、停止する 0:free,1:disable,2:stop, 3:position固定 75 | #dev.run_at_velocity(utils.rpm2rad_per_sec(20)) 76 | #dev.reset_all_registers() 77 | sleep(1) 78 | """ 79 | Exit with key input 80 | """ 81 | while True: 82 | dev.run_at_velocity(utils.rpm2rad_per_sec(20)) 83 | sleep(0.05) -------------------------------------------------------------------------------- /examples/ble-get-motor-Informations.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | 9 | 10 | import sys 11 | import os 12 | import pathlib 13 | from time import sleep 14 | from concurrent.futures import ThreadPoolExecutor 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import blecontroller 20 | 21 | """ 22 | ---------------------- 23 | モーターに接続し、各種情報の取得 24 | ---------------------- 25 | """ 26 | 27 | def get_motor_informations(): 28 | while True: 29 | if dev: 30 | print("\033[3;2H\033[2K") 31 | sys.stdout.flush() 32 | print('status {} '.format(dev.read_status())) 33 | sys.stdout.flush() 34 | 35 | print("\033[8;2H\033[2K") 36 | sys.stdout.flush() 37 | print('measurement {} '.format(dev.read_motor_measurement())) 38 | sys.stdout.flush() 39 | 40 | print("\033[12;2H\033[2K") 41 | sys.stdout.flush() 42 | print('imu_measurement {} '.format(dev.read_imu_measurement())) 43 | sys.stdout.flush() 44 | sleep(0.5) 45 | 46 | #接続 47 | dev=blecontroller.BLEController("d1:5a:fa:a7:d9:5d")#モーターのMACアドレス 参照 ble-simple-connection.py 48 | dev.set_led(2,255,255,0) 49 | dev.enable_continual_imu_measurement()#IMUは通知をOnにする必要がある 50 | sleep(0.5) 51 | 52 | #一定間隔で取得 53 | executor = ThreadPoolExecutor(max_workers=2) 54 | res = executor.submit(get_motor_informations) 55 | 56 | 57 | """ 58 | Exit with key input 59 | """ 60 | os.system('clear') 61 | for i in range(20): 62 | print("　　　　　　　") 63 | print("\033[17;2H") 64 | print("---------------------------------------") 65 | print("\033[3;2H\033[2K") 66 | sys.stdout.flush() 67 | sleep(0.5) 68 | while True: 69 | print("\033[18;2H") 70 | sys.stdout.flush() 71 | print("---------------------------------------") 72 | if sys.version_info<(3,0): 73 | inp = raw_input('Exit:[key input] >>') 74 | else: 75 | inp = input('Exit:[key input] >>') 76 | if inp !=None: 77 | dev.set_led(1, 100, 100, 100) 78 | dev.disable_continual_imu_measurement() 79 | dev.disconnect() 80 | break 81 | -------------------------------------------------------------------------------- /examples/windows_examples/baudrate_run.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import serial 13 | import msvcrt 14 | import serial.tools.list_ports 15 | from time import sleep 16 | 17 | current_dir = pathlib.Path(__file__).resolve().parent 18 | sys.path.append( str(current_dir) + '/../../' ) 19 | 20 | from pykeigan import usbcontroller 21 | from pykeigan import utils 22 | 23 | """ 24 | ---------------------- 25 | Run after Change of Baud Rate 26 | You need to execute "baudrate_change.py" before executing this sample 27 | Choose COM port connected to KeiganMotor, and 28 | Choose baud rate that you set by "baudrate_change.py". 29 | 30 | ボーレート変更後の動作確認 31 | 本サンプル実行前に、"baudrate_change.py" を実行してボーレートを変更すること 32 | COMポートと、"baudrate_change.py" で設定したボーレートを選択する 33 | ---------------------- 34 | 35 | """ 36 | # This sample requires KM-1 firmware version more than 2.37 37 | 38 | baud_rates = {0:"115200",1:"230400",2:"250000",3:"460800",4:"921600",5:"1000000"} 39 | 40 | def select_port(): 41 | print('Available COM ports list') 42 | 43 | portlist = serial.tools.list_ports.comports() 44 | 45 | if not portlist: 46 | print('No available port') 47 | sys.exit() 48 | 49 | print('i : name') 50 | print('--------') 51 | for i, port in enumerate(portlist): 52 | print(i, ':', port.device) 53 | 54 | print('- Enter the port number (0~)') 55 | portnum = input() 56 | portnum = int(portnum) 57 | 58 | portdev = None 59 | if portnum in range(len(portlist)): 60 | portdev = portlist[portnum].device 61 | 62 | print('Conncted to', portdev) 63 | 64 | return portdev 65 | 66 | 67 | def baud_rate_setting(): 68 | print('Select baud rate') 69 | print('--------') 70 | print('0: 115200') 71 | print('1: 230400') 72 | print('2: 250000') 73 | print('3: 460800') 74 | print('4: 921600') 75 | print('5: 1000000 (1M)') 76 | print('--------') 77 | num = int(input()) 78 | while num < 0 or num > 5: 79 | print('Invalid value!') 80 | num = int(input()) 81 | return num 82 | 83 | port = select_port() 84 | baud_rate = baud_rates[baud_rate_setting()] 85 | dev=usbcontroller.USBController(port,baud=baud_rate) 86 | dev.enable_action() #5: 1Mbps 87 | dev.run_at_velocity(utils.rpm2rad_per_sec(100)) 88 | 89 | -------------------------------------------------------------------------------- /examples/windows_examples/taskset_move.py: -------------------------------------------------------------------------------- 1 | import msvcrt 2 | from time import sleep 3 | 4 | current_dir = pathlib.Path(__file__).resolve().parent 5 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 6 | 7 | from pykeigan import usbcontroller 8 | from pykeigan import utils 9 | 10 | # How to use (this sample was made for Windows) 11 | # (this sample was made for Windows) 12 | # 13 | # (1) Define port 'COM?' that connected to KeiganMotor (You can see by DeviceManager) 14 | # (2) Set index to record and do taskset (selectable from 0 to 49) 15 | # (3) Set repeat to repeat when doing taskset (0~, 0 will repeat infinitely) 16 | # (4) Excute this file 17 | # (5) Press 'r' to record my_taskset in KeiganMotor 18 | # (6) Press 'd' to do taskset 19 | 20 | 21 | # Select port connecting to KeiganMotor 22 | port = 'COM7' 23 | 24 | index = 1 # index to record and do taskset 25 | repeat = 2 # repeating number to do taskset 26 | 27 | # Edit this function to record your taskset 28 | # (Recorded in KeiganMotor) 29 | def my_taskset(): 30 | dev.set_speed(utils.rpm2rad_per_sec(5)) 31 | dev.set_led(1, 0, 200, 0) 32 | dev.move_by_dist(utils.deg2rad(30)) 33 | dev.wait_queue(2000) # wait for 5000 ms 34 | dev.stop_motor() 35 | 36 | def record_taskset(): 37 | dev.erase_taskset(index) 38 | sleep(0.5) 39 | dev.start_recording_taskset(index) 40 | my_taskset() 41 | dev.stop_recording_taskset() 42 | 43 | def do_taskset(): 44 | dev.enable_action() 45 | dev.start_doing_taskset(index, repeat) 46 | 47 | # Log callback 48 | def on_motor_log_cb(log): 49 | if log['error_codes']!='KM_SUCCESS': 50 | print('log {} '.format(log)) 51 | 52 | 53 | if __name__ == '__main__': 54 | dev = usbcontroller.USBController(port) 55 | dev.on_motor_log_cb=on_motor_log_cb 56 | try: 57 | while True: 58 | sleep(0.01) 59 | if msvcrt.kbhit(): 60 | c = msvcrt.getwch() 61 | # print(c) 62 | 63 | if c == 'r': 64 | print('Record taskset index: %d' %index) 65 | record_taskset() 66 | 67 | elif c == 'd': 68 | print('Do taskset index: %d, repeat: %d' %(index, repeat)) 69 | do_taskset() 70 | 71 | except KeyboardInterrupt: 72 | if dev: 73 | dev.disable_action() 74 | print('Ctrl-C') 75 | -------------------------------------------------------------------------------- /examples/readme.md: -------------------------------------------------------------------------------- 1 | ## python用サンプルコード 2 | Github 3 | 4 | https://github.com/keigan-motor/pykeigan_motor/examples 5 | 6 | ## Requirements 7 | + python >= 3.5 (recommended) or 2.6 8 | + pyserial >= 3.4 9 | + bluepy >= 1.1.4 10 | 11 | ## BLEサンプル 12 | 13 | + KM1Scan.py 14 | 15 | Bluetooth接続で使用するKeiganMotorのモーターのMACアドレスを検出するのに使用 16 | 17 | BluetoothのScanner実行には管理者権限が必要な為、sudoで実行 18 | 19 | $sudo python KM1Scan.py 20 | 21 | + ble-scanner-connection.py 22 | 23 | BluetoothのScanner実行には管理者権限が必要な為、sudoで実行 24 | 25 | $sudo python3 ble-scanner-connection.py 26 | 27 | + ble-simple-connection.py 28 | 29 | 実行前にモーターのMACアドレスを設定する必要があります。 30 | 31 | $python3 ble-simple-connection.py 32 | 33 | + ble-rotate-the-motor.py 34 | 35 | 実行前にモーターのMACアドレスを設定する必要があります。 36 | 37 | $python3 ble-rotate-the-motor.py 38 | 39 | + ble-get-motor-Informations.py 40 | 41 | モーターの回転情報の取得 42 | 43 | 実行前にモーターのMACアドレスを設定する必要があります。 44 | 45 | $python3 ble-get-motor-Informations.py 46 | 47 | 48 | MACアドレスはKM1Scan.pyを使用するか"nRF Connect"を使用して調べて下さい。 49 | 50 | nRF Connect 51 | https://play.google.com/store/apps/details?id=no.nordicsemi.android.mcp 52 | 53 | Bluetoothでモーターが検出されない場合は、モーターの"インターフェイスのリセット"を実行して下さい。 54 | 55 | https://document.keigan-motor.com/basic/reset 56 | 57 | 58 | ## USBサンプル 59 | 60 | 実行前にデバイスファイルを指定する必要があります ex'/dev/ttyUSB0' 61 | 62 | + usb-simple-connection.py 63 | 64 | 基本的な接続 65 | 66 | $python3 usb-simple-connection.py 67 | 68 | + usb-actuator.py 69 | 70 | モーターの往復運動 71 | 72 | $python3 usb-actuator.py 73 | 74 | + usb-button_trigger.py 75 | 76 | KeiganMotor 本体のボタンイベント取得 77 | 78 | $python3 usb-button_trigger.py 79 | 80 | + usb-get-motor-Informations.py 81 | 82 | モーターの回転情報の取得 83 | 84 | $python3 usb-get-motor-Informations.py 85 | 86 | + usb-position-control.py 87 | 88 | モーターの相対・絶対移動 89 | 90 | $python3 usb-position-control.py 91 | 92 | + usb-rotate-the-motor.py 93 | 94 | モーターを5rpmで 正転(10秒) -> 逆転(10秒) -> 停止(トルクあり) -> 停止(トルク無し) 95 | 96 | $python3 usb-rotate-the-motor.py 97 | 98 | 99 | + usb-teaching-control.py 100 | 101 | ティーチング記録・再生 102 | 103 | $python3 usb-teaching-control.py 104 | 105 | + usb-torque-control.py 106 | 107 | トルク制御。モーターを手で回して行くとトルクが加算される。 108 | 109 | $python3 usb-torque-control.py 110 | -------------------------------------------------------------------------------- /examples/usb-move_wait_queue.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Apr 2021 4 | 5 | @author: Keigan Inc. 6 | """ 7 | import argparse 8 | import signal 9 | import sys 10 | import os 11 | import pathlib 12 | from time import sleep 13 | 14 | from concurrent.futures import ThreadPoolExecutor 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import usbcontroller 20 | from pykeigan import utils 21 | 22 | parser = argparse.ArgumentParser(description="モーター動作 相対移動と絶対位置移動") 23 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 24 | args = parser.parse_args() 25 | 26 | os.system('clear') 27 | for i in range(10): 28 | print("　　　　　　　") 29 | print("\033[9;2H","---------------------------------------", "\033[2;2H\033[2K") 30 | sys.stdout.flush() 31 | 32 | """ 33 | ---------------------- 34 | モーターに接続し、各種情報の取得 35 | ---------------------- 36 | """ 37 | ##モーター回転情報callback 38 | def on_motor_measurement_cb(measurement): 39 | print("\033[2;2H\033[2K") 40 | print('measurement {} '.format(measurement)) 41 | sys.stdout.flush() 42 | 43 | ##ログ情報callback 44 | def on_motor_log_cb(log): 45 | print("\033[5;2H\033[2K") 46 | sys.stdout.flush() 47 | print('log {} '.format(log)) 48 | sys.stdout.flush() 49 | 50 | #接続 51 | dev = usbcontroller.USBController(args.port) 52 | #dev.on_motor_log_cb = on_motor_log_cb 53 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 54 | 55 | dev.enable_action() 56 | dev.set_speed(utils.rpm2rad_per_sec(100)) 57 | 58 | # 位置到達の判定条件を決める 59 | # def set_notify_pos_arrival_settings 60 | # 引数1: python側に通知するかどうか 61 | # 引数2: 到達しているとみなす許容誤差 ±θ [rad] 62 | # 引数3: 到達している状態が t[ms] 継続したら到達完了とみなす 63 | # （例）誤差 ± 1.0[deg] で到達判定OK 到着判定が 1ms 継続で、モーターから位置到達通知 64 | dev.set_notify_pos_arrival_settings(True, utils.deg2rad(1), 1) 65 | 66 | dev.set_curve_type(0) 67 | 68 | # move_to_pos_wait / move_by_dist_wait コマンドはファームウェア ver 2.66以降対応 69 | 70 | ## 絶対位置へ移動（到着まで命令を保持） 71 | dev.move_to_pos_wait(utils.deg2rad(0)) 72 | dev.move_to_pos_wait(utils.deg2rad(90)) 73 | dev.move_to_pos_wait(utils.deg2rad(0)) 74 | dev.move_to_pos_wait(utils.deg2rad(-90)) 75 | dev.move_to_pos_wait(utils.deg2rad(0)) 76 | 77 | ## 相対位置へ移動（到着まで命令を保持） 78 | # dev.move_by_dist_wait(utils.deg2rad(90)) 79 | # dev.move_by_dist_wait(utils.deg2rad(-90)) 80 | # dev.move_by_dist_wait(utils.deg2rad(90)) 81 | # dev.move_by_dist_wait(utils.deg2rad(-90)) 82 | 83 | try: 84 | while True: 85 | pass # ここに、Ctrl-C で止めたい処理を書く 86 | 87 | except KeyboardInterrupt: 88 | if dev: 89 | dev.disable_action() 90 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/windows_examples/keigan_motor_initial_setting.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # coding:utf-8 3 | import argparse 4 | import sys 5 | import pathlib 6 | import msvcrt 7 | import serial.tools.list_ports 8 | from time import sleep 9 | import time 10 | 11 | current_dir = pathlib.Path(__file__).resolve().parent 12 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 13 | 14 | from pykeigan import utils 15 | from pykeigan import usbcontroller 16 | 17 | # '0: 115200' 18 | # '1: 230400' 19 | # '2: 250000' 20 | # '3: 460800' 21 | # '4: 921600' 22 | # '5: 1000000 (1M)' 23 | 24 | baud_list = [115200, 230400, 250000, 460800, 921600, 1000000] 25 | 26 | def current_baud_rate(): 27 | print('Select current baud rate (bps) of KeiganMotor side') 28 | print("0: 115200, 1: 230400, 2: 250000, 3: 460800, 4: 921600, 5: 1000000 (1M)") 29 | print("Default is 0: 115200") 30 | print('- Enter the port number (0~5)') 31 | 32 | baud_num = int(input()) 33 | 34 | if baud_num > 5 or baud_num < 0: 35 | baud_num = 0 36 | 37 | baud_rate = baud_list[baud_num] 38 | 39 | return baud_rate 40 | 41 | 42 | def select_port(): 43 | print('Available COM ports list') 44 | 45 | portlist = serial.tools.list_ports.comports() 46 | 47 | if not portlist: 48 | print('No available port') 49 | sys.exit() 50 | 51 | print('i : name') 52 | print('--------') 53 | for i, port in enumerate(portlist): 54 | print(i, ':', port.device) 55 | 56 | print('- Enter the port number (0~)') 57 | portnum = input() 58 | portnum = int(portnum) 59 | 60 | portdev = None 61 | if portnum in range(len(portlist)): 62 | portdev = portlist[portnum].device 63 | 64 | print('Connected to', portdev) 65 | 66 | return portdev 67 | 68 | dev = usbcontroller.USBController(select_port(), baud=current_baud_rate()) 69 | 70 | # '0: 115200' 71 | # '1: 230400' 72 | # '2: 250000' 73 | # '3: 460800' 74 | # '4: 921600' 75 | # '5: 1000000 (1M)' 76 | 77 | # '0: 115200' 78 | # '1: 230400' 79 | # '2: 250000' 80 | # '3: 460800' 81 | # '4: 921600' 82 | # '5: 1000000 (1M)' 83 | # 設定したいボーレートに対応する整数を入れてください 84 | dev.set_baud_rate(5) #5: 1Mbps 85 | 86 | # 0: 2 [ms] 87 | # 1: 5 [ms] 88 | # 2: 10 [ms] 89 | # 3: 20 [ms] 90 | # 4: 50 [ms] 91 | # 5: 100 [ms] 92 | # 6: 200 [ms] 93 | # 7: 500 [ms] 94 | # 8: 1000 [ms] 95 | # 9: 2000 [ms] 96 | # 10: 5000 [ms] 97 | # 11: 10000 [ms] 98 | # 設定したい測定値送信間隔に対応する整数を入れてください 99 | dev.set_motor_measurement_interval(0) 100 | dev.set_led(1, 255, 0, 0) 101 | dev.save_all_registers() 102 | print("dev.save_all_registers()") 103 | time.sleep(2) 104 | dev.set_led(1, 255, 255, 0) 105 | dev.reboot() -------------------------------------------------------------------------------- /examples/windows_examples/move_wait_queue.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Apr 24 2021 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import utils 20 | from pykeigan import usbcontroller 21 | 22 | def select_port(): 23 | print('Available COM ports list') 24 | 25 | portlist = serial.tools.list_ports.comports() 26 | 27 | if not portlist: 28 | print('No available port') 29 | sys.exit() 30 | 31 | print('i : name') 32 | print('--------') 33 | for i, port in enumerate(portlist): 34 | print(i, ':', port.device) 35 | 36 | print('- Enter the port number (0~)') 37 | portnum = input() 38 | portnum = int(portnum) 39 | 40 | portdev = None 41 | if portnum in range(len(portlist)): 42 | portdev = portlist[portnum].device 43 | 44 | print('Connected to', portdev) 45 | 46 | return portdev 47 | 48 | 49 | 50 | # ログ情報callback 51 | def on_motor_log_cb(log): 52 | print('log {} '.format(log)) 53 | 54 | if log['error_codes'] == 'KM_SUCCESS': 55 | print('Command Success') 56 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 57 | print('Position Arrival Success') 58 | 59 | 60 | # モーター回転情報callback 61 | def on_motor_measurement_cb(measurement): 62 | print("\r"+'measurement {} '.format(measurement), end="") 63 | 64 | 65 | dev = usbcontroller.USBController(select_port()) 66 | #dev.on_motor_log_cb = on_motor_log_cb 67 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 68 | 69 | dev.enable_action() 70 | dev.set_speed(utils.rpm2rad_per_sec(100)) 71 | 72 | # 位置到達の判定条件を決める 73 | # def set_notify_pos_arrival_settings 74 | # 引数1: python側に通知するかどうか 75 | # 引数2: 到達しているとみなす許容誤差 ±θ [rad] 76 | # 引数3: 到達している状態が t[ms] 継続したら到達完了とみなす 77 | # （例）誤差 ± 1.0[deg] で到達判定OK 到着判定が 1ms 継続で、モーターから位置到達通知 78 | dev.set_notify_pos_arrival_settings(True, utils.deg2rad(1), 1) 79 | 80 | dev.set_curve_type(0) 81 | 82 | # move_to_pos_wait / move_by_dist_wait コマンドはファームウェア ver 2.66以降対応 83 | 84 | ## 絶対位置へ移動（到着まで命令を保持） 85 | dev.move_to_pos_wait(utils.deg2rad(0)) 86 | dev.move_to_pos_wait(utils.deg2rad(90)) 87 | dev.move_to_pos_wait(utils.deg2rad(0)) 88 | dev.move_to_pos_wait(utils.deg2rad(-90)) 89 | dev.move_to_pos_wait(utils.deg2rad(0)) 90 | 91 | ## 相対位置へ移動（到着まで命令を保持） 92 | # dev.move_by_dist_wait(utils.deg2rad(90)) 93 | # dev.move_by_dist_wait(utils.deg2rad(-90)) 94 | # dev.move_by_dist_wait(utils.deg2rad(90)) 95 | # dev.move_by_dist_wait(utils.deg2rad(-90)) 96 | 97 | try: 98 | while True: 99 | pass # ここに、Ctrl-C で止めたい処理を書く 100 | 101 | except KeyboardInterrupt: 102 | if dev: 103 | dev.disable_action() 104 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/windows_examples/pos_arrival_test.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import serial 13 | import msvcrt 14 | import serial.tools.list_ports 15 | from time import sleep 16 | 17 | current_dir = pathlib.Path(__file__).resolve().parent 18 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 19 | 20 | from pykeigan import utils 21 | from pykeigan import usbcontroller 22 | 23 | def select_port(): 24 | print('Available COM ports list') 25 | 26 | portlist = serial.tools.list_ports.comports() 27 | 28 | if not portlist: 29 | print('No available port') 30 | sys.exit() 31 | 32 | print('i : name') 33 | print('--------') 34 | for i, port in enumerate(portlist): 35 | print(i, ':', port.device) 36 | 37 | print('- Enter the port number (0~)') 38 | portnum = input() 39 | portnum = int(portnum) 40 | 41 | portdev = None 42 | if portnum in range(len(portlist)): 43 | portdev = portlist[portnum].device 44 | 45 | print('Connected to', portdev) 46 | 47 | return portdev 48 | 49 | 50 | # isGo means on the way, !isGo means on the way back. 51 | isGo = True 52 | 53 | def go_round(): 54 | global isGo 55 | if isGo: 56 | print('Go to the target') 57 | dev.move_to_pos(utils.deg2rad(1080)) 58 | else: 59 | print('Go back to Zero') 60 | dev.move_to_pos(utils.deg2rad(0)) 61 | 62 | # ログ情報callback 63 | def on_motor_log_cb(log): 64 | global isGo 65 | print('log {} '.format(log)) 66 | 67 | if log['error_codes'] == 'KM_SUCCESS': 68 | print('Command Success') 69 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 70 | print('Position Arrival Success') 71 | isGo = not isGo 72 | go_round() 73 | 74 | 75 | # モーター再接続 callback 76 | def on_motor_reconnection_cb(cnt): 77 | go_round() 78 | 79 | # モーター回転情報callback 80 | def on_motor_measurement_cb(measurement): 81 | #print("\033[2;2H\033[2K", end="") 82 | # , end="", flush=True) 83 | print("\r"+'measurement {} '.format(measurement), end="") 84 | #print("\033[20;2H", end="",flush=True) 85 | 86 | 87 | dev = usbcontroller.USBController(select_port()) 88 | dev.on_motor_log_cb = on_motor_log_cb 89 | #dev.on_motor_connection_error_cb = on_motor_connection_error_cb 90 | dev.on_motor_reconnection_cb = on_motor_reconnection_cb 91 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 92 | dev.enable_action() 93 | dev.set_pos_control_threshold(utils.deg2rad(2)) # 位置のPID制御有効区間を大きくしておく 0.8[deg] -> 2.0[deg] 94 | dev.set_notify_pos_arrival_settings(True, 0.00872665, 200) # 0.00872665[rad] = 0.5[deg] に 到達して 200[ms] 経過で モーターから位置到達通知 95 | dev.set_speed(utils.rpm2rad_per_sec(200)) 96 | go_round() 97 | 98 | """ 99 | Exit with key input 100 | """ 101 | 102 | while True: 103 | sleep(0.1) 104 | -------------------------------------------------------------------------------- /examples/windows_examples/move_to_continious.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import utils 20 | from pykeigan import usbcontroller 21 | 22 | def select_port(): 23 | print('Available COM ports list') 24 | 25 | portlist = serial.tools.list_ports.comports() 26 | 27 | if not portlist: 28 | print('No available port') 29 | sys.exit() 30 | 31 | print('i : name') 32 | print('--------') 33 | for i, port in enumerate(portlist): 34 | print(i, ':', port.device) 35 | 36 | print('- Enter the port number (0~)') 37 | portnum = input() 38 | portnum = int(portnum) 39 | 40 | portdev = None 41 | if portnum in range(len(portlist)): 42 | portdev = portlist[portnum].device 43 | 44 | print('Connected to', portdev) 45 | 46 | return portdev 47 | 48 | 49 | 50 | # ログ情報callback 51 | def on_motor_log_cb(log): 52 | print('log {} '.format(log)) 53 | 54 | if log['error_codes'] == 'KM_SUCCESS': 55 | print('Command Success') 56 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 57 | print('Position Arrival Success') 58 | 59 | 60 | 61 | 62 | # モーター回転情報callback 63 | def on_motor_measurement_cb(measurement): 64 | print("\r"+'measurement {} '.format(measurement), end="") 65 | 66 | 67 | dev = usbcontroller.USBController(select_port()) 68 | #dev.on_motor_log_cb = on_motor_log_cb 69 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 70 | dev.enable_action() 71 | dev.set_curve_type(10) # 10: ダイレクト位置制御（速度制限、カーブなしで位置制御を行う。振動対策） 72 | dev.set_speed_i(0) 73 | #dev.set_pos_control_threshold(utils.deg2rad(2)) 74 | #dev.set_position_p(10) 75 | #dev.set_position_i(2) 76 | #dev.set_speed(utils.rpm2rad_per_sec(20)) 77 | # 連続で動作命令を送る場合、位置到達時の通知設定をOFFとする必要がある 78 | # dev.set_notify_pos_arrival_settings(False, 0.00872665, 200) # 第1引数 False で無効化 79 | dev.set_safe_run_settings(True, 100, 3) # 第1引数が True の場合、5000[ms]以内に次の動作命令が来ないと、停止する 0:free,1:disable,2:stop, 3:position固定 80 | 81 | # 最初 0[deg] へ移動 82 | targetPos = 0 83 | inc = 0 84 | """ 85 | Exit with key input 86 | """ 87 | try: 88 | while True: 89 | dev.move_to_pos(utils.deg2rad(targetPos+inc)) 90 | inc += 0.1 # 少しずつ目標位置をずらしていく。位置固定の場合はこの行をコメントアウト 91 | sleep(0.01) 92 | if msvcrt.kbhit(): 93 | c = msvcrt.getwch() 94 | print(c) 95 | if c == 'f': # 1080[deg] へ移動 96 | targetPos = 1080 97 | elif c == 'b': # 0[deg] へ移動 98 | targetPos = 0 99 | 100 | 101 | 102 | except KeyboardInterrupt: 103 | if dev: 104 | dev.disable_action() 105 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/usb-get-motor-Info-time.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import signal 10 | import sys 11 | import os 12 | import pathlib 13 | from time import sleep 14 | from concurrent.futures import ThreadPoolExecutor 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import usbcontroller 20 | 21 | parser = argparse.ArgumentParser(description='モーターに接続し、各種情報の取得') 22 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 23 | args = parser.parse_args() 24 | 25 | os.system('clear') 26 | for i in range(24): 27 | print("　　　　　　　") 28 | 29 | print("\033[19;2H","---------------------------------------", "\033[2;2H\033[2K") 30 | sys.stdout.flush() 31 | 32 | """ 33 | ---------------------- 34 | モーターに接続し、各種情報の取得 35 | ---------------------- 36 | """ 37 | ##モーター回転情報callback 38 | def on_motor_measurement_cb(measurement): 39 | print("\033[2;2H\033[2K") 40 | print('measurement {} '.format(measurement)) 41 | sys.stdout.flush() 42 | 43 | ##IMU情報callback 44 | def on_motor_imu_measurement_cb(measurement): 45 | print("\033[6;2H\033[2K") 46 | print('imu_measurement {} '.format(measurement)) 47 | sys.stdout.flush() 48 | 49 | ##ログ情報callback 50 | def on_motor_log_cb(log): 51 | print("\033[12;2H\033[2K") 52 | sys.stdout.flush() 53 | print('log {} '.format(log)) 54 | sys.stdout.flush() 55 | 56 | ##エラー情報callback 57 | def on_motor_connection_error_cb(e): 58 | print("\033[16;2H\033[2K") 59 | sys.stdout.flush() 60 | print('error {} '.format(e)) 61 | sys.stdout.flush() 62 | 63 | 64 | #接続 65 | #dev=usbcontroller.USBController('/dev/ttyUSB0',False)#モーターのアドレス 参照 usb-simple-connection.py 66 | dev=usbcontroller.USBController(args.port,False)#モーターのアドレス 参照 usb-simple-connection.py 67 | dev.on_motor_measurement_value_cb=on_motor_measurement_cb 68 | dev.on_motor_imu_measurement_cb=on_motor_imu_measurement_cb 69 | dev.on_motor_log_cb=on_motor_log_cb 70 | dev.on_motor_connection_error_cb=on_motor_connection_error_cb 71 | 72 | dev.enable_continual_imu_measurement()#IMUはデフォルトでOFFの為、取得する場合Onにする 73 | 74 | #モーター動作 75 | dev.set_led(2,255,255,0) 76 | sleep(3) 77 | dev.enable_action() 78 | dev.set_speed(1.0) 79 | dev.run_forward() 80 | sleep(10) 81 | dev.disable_action() 82 | 83 | 84 | """ 85 | Exit with key input 86 | """ 87 | 88 | sleep(0.5) 89 | while True: 90 | print("\033[20;2H") 91 | sys.stdout.flush() 92 | print("---------------------------------------") 93 | if sys.version_info <(3,0): 94 | inp = raw_input('Exit:[key input] >>') 95 | else: 96 | inp = input('Exit:[key input] >>') 97 | if inp !=None: 98 | dev.set_led(1, 100, 100, 100) 99 | dev.disable_action() 100 | dev.disconnect() 101 | break 102 | -------------------------------------------------------------------------------- /examples/usb-actuator.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import signal 10 | import sys 11 | import os 12 | import pathlib 13 | from time import sleep 14 | 15 | from concurrent.futures import ThreadPoolExecutor 16 | 17 | current_dir = pathlib.Path(__file__).resolve().parent 18 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 19 | 20 | from pykeigan import usbcontroller 21 | from pykeigan import utils 22 | 23 | parser = argparse.ArgumentParser(description='モーター動作 絶対位置往復運動') 24 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 25 | args = parser.parse_args() 26 | 27 | os.system('clear') 28 | for i in range(10): 29 | print("　　　　　　　") 30 | print("\033[9;2H","---------------------------------------", "\033[2;2H\033[2K") 31 | sys.stdout.flush() 32 | 33 | """ 34 | ---------------------- 35 | モーター接続、各種情報の取得 36 | ---------------------- 37 | """ 38 | ##モーター回転情報callback 39 | def on_motor_measurement_cb(measurement): 40 | print("\033[2;2H\033[2K") 41 | print('measurement {} '.format(measurement)) 42 | sys.stdout.flush() 43 | 44 | ##ログ情報callback 45 | def on_motor_log_cb(log): 46 | print("\033[5;2H\033[2K") 47 | sys.stdout.flush() 48 | print('log {} '.format(log)) 49 | sys.stdout.flush() 50 | 51 | #接続 52 | dev=usbcontroller.USBController(args.port,False) 53 | dev.on_motor_measurement_value_cb=on_motor_measurement_cb 54 | dev.on_motor_log_cb=on_motor_log_cb 55 | 56 | 57 | """ 58 | ---------------------- 59 | モーター動作 絶対位置往復運動 60 | ---------------------- 61 | """ 62 | is_forward=False 63 | cnt=0 64 | #位置を監視して反転 65 | def direction_measurement_cb(measurement): 66 | on_motor_measurement_cb(measurement) 67 | global is_forward,cnt 68 | 69 | if is_forward and measurement['position'] > (utils.deg2rad(720) - 0.02): 70 | is_forward=False 71 | cnt += 1 72 | if cnt > 4: 73 | dev.disable_action() 74 | else: 75 | dev.set_led(2, 255, 0, 255) 76 | dev.move_to_pos(utils.deg2rad(0)) 77 | 78 | if not is_forward and measurement['position']<(0+0.02): 79 | is_forward=True 80 | dev.set_led(2, 0, 255, 255) 81 | dev.move_to_pos(utils.deg2rad(720)) # 2回転 82 | 83 | 84 | 85 | dev.set_speed(utils.rpm2rad_per_sec(20)) 86 | dev.preset_position(0)#現在位置の座標を0に設定 87 | dev.enable_action() 88 | dev.on_motor_measurement_value_cb=direction_measurement_cb 89 | 90 | 91 | """ 92 | Exit with key input 93 | """ 94 | 95 | sleep(0.5) 96 | while True: 97 | print("\033[10;2H") 98 | sys.stdout.flush() 99 | print("---------------------------------------") 100 | if sys.version_info <(3,0): 101 | inp = raw_input('Exit:[key input] >>') 102 | else: 103 | inp = input('Exit:[key input] >>') 104 | if inp !=None: 105 | dev.set_led(1, 100, 100, 100) 106 | dev.disable_action() 107 | dev.disconnect() 108 | break 109 | -------------------------------------------------------------------------------- /examples/usb-get-motor-Informations.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import signal 10 | import sys 11 | import os 12 | import pathlib 13 | from time import sleep 14 | from concurrent.futures import ThreadPoolExecutor 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import usbcontroller 20 | 21 | parser = argparse.ArgumentParser(description='モーターに接続し、各種情報の取得') 22 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 23 | args = parser.parse_args() 24 | 25 | os.system('clear') 26 | for i in range(24): 27 | print("　　　　　　　") 28 | 29 | print("\033[19;2H","---------------------------------------", "\033[2;2H\033[2K") 30 | sys.stdout.flush() 31 | 32 | """ 33 | ---------------------- 34 | モーターに接続し、各種情報の取得 35 | ---------------------- 36 | """ 37 | ##モーター回転情報callback 38 | def on_motor_measurement_cb(measurement): 39 | print("\033[2;2H\033[2K") 40 | print('measurement {} '.format(measurement)) 41 | sys.stdout.flush() 42 | 43 | ##IMU情報callback 44 | def on_motor_imu_measurement_cb(measurement): 45 | print("\033[6;2H\033[2K") 46 | print('imu_measurement {} '.format(measurement)) 47 | sys.stdout.flush() 48 | 49 | ##ログ情報callback 50 | def on_motor_log_cb(log): 51 | print("\033[12;2H\033[2K") 52 | sys.stdout.flush() 53 | print('log {} '.format(log)) 54 | sys.stdout.flush() 55 | 56 | ##エラー情報callback 57 | def on_motor_connection_error_cb(e): 58 | print("\033[16;2H\033[2K") 59 | sys.stdout.flush() 60 | print('error {} '.format(e)) 61 | sys.stdout.flush() 62 | 63 | 64 | #接続 65 | #dev=usbcontroller.USBController('/dev/ttyUSB0',False)#モーターのアドレス 参照 usb-simple-connection.py 66 | dev=usbcontroller.USBController(args.port,False)#モーターのアドレス 参照 usb-simple-connection.py 67 | dev.on_motor_measurement_value_cb=on_motor_measurement_cb 68 | dev.on_motor_imu_measurement_cb=on_motor_imu_measurement_cb 69 | dev.on_motor_log_cb=on_motor_log_cb 70 | dev.on_motor_connection_error_cb=on_motor_connection_error_cb 71 | 72 | dev.enable_continual_imu_measurement()#IMUはデフォルトでOFFの為、取得する場合Onにする 73 | 74 | # ビットフラグ 0x40 でモーターの時刻送信を有効化 ※ モーターFW ver 2.62以降対応 75 | dev.set_motor_measurement_settings(5) 76 | 77 | #モーター動作 78 | dev.set_led(2,255,255,0) 79 | sleep(3) 80 | dev.enable_action() 81 | dev.set_speed(1.0) 82 | dev.run_forward() 83 | sleep(10) 84 | dev.disable_action() 85 | 86 | 87 | """ 88 | Exit with key input 89 | """ 90 | 91 | sleep(0.5) 92 | while True: 93 | print("\033[20;2H") 94 | sys.stdout.flush() 95 | print("---------------------------------------") 96 | if sys.version_info <(3,0): 97 | inp = raw_input('Exit:[key input] >>') 98 | else: 99 | inp = input('Exit:[key input] >>') 100 | if inp !=None: 101 | dev.set_led(1, 100, 100, 100) 102 | dev.disable_action() 103 | dev.disconnect() 104 | break 105 | -------------------------------------------------------------------------------- /examples/usb-sine-wave.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | from argparse import RawTextHelpFormatter 11 | import sys 12 | import pathlib 13 | from time import sleep 14 | import signal # タイマーで定期実行するためのシグナルライブラリ（高精度に定期実行できる） 15 | import math 16 | import numpy as np 17 | 18 | import matplotlib.pyplot as plt # グラフ作成のため 19 | 20 | current_dir = pathlib.Path(__file__).resolve().parent 21 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 22 | 23 | from pykeigan import utils 24 | from pykeigan import usbcontroller 25 | 26 | 27 | 28 | 29 | 30 | 31 | # ログ情報callback 32 | def on_motor_log_cb(log): 33 | print('log {} '.format(log)) 34 | 35 | if log['error_codes'] == 'KM_SUCCESS': 36 | print('Command Success') 37 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 38 | print('Position Arrival Success') 39 | 40 | 41 | data = [] 42 | maxNum = 500 43 | 44 | # モーター回転情報callback 45 | def on_motor_measurement_cb(measurement): 46 | global data, maxNum 47 | print("\r"+'measurement {} '.format(measurement)) 48 | 49 | 50 | 51 | # 正弦波 52 | f = 0.5 # 周波数 [s] 53 | ampDegree = 80 # 振幅 [degree] 54 | t = 0 55 | res = 0.01 # 時間分解能 [s] 56 | oneRound = f/res # 1周分 57 | 58 | 59 | # シグナルライブラリ（schedular関数を定期的に呼ぶ）のタイマースタート 60 | def timer_start(): 61 | signal.setitimer(signal.ITIMER_REAL, 1, res) # 第2引数の1は、1秒後にスタート、第3引数は、タイマー間隔 62 | 63 | # シグナルライブラリ（schedular関数を定期的に呼ぶ）のタイマーストップ 64 | def timer_stop(): 65 | signal.setitimer(signal.ITIMER_REAL, 0, res) # 第2引数を0にすると、タイマーは停止する決まり 66 | 67 | 68 | 69 | 70 | # シグナルライブラリで定期実行される関数。 71 | def scheduler(arg1, args2): 72 | global ampDegree, t, res, oneRound 73 | target = utils.deg2rad(ampDegree * math.sin(t * 2 * math.pi / oneRound)) 74 | dev.move_to_pos(target) 75 | t += 1 76 | 77 | 78 | description="" 79 | parser = argparse.ArgumentParser(description=description,formatter_class=RawTextHelpFormatter) 80 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 81 | args = parser.parse_args() 82 | 83 | dev=usbcontroller.USBController(args.port) 84 | #dev.on_motor_log_cb = on_motor_log_cb 85 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 86 | dev.enable_action() 87 | dev.set_speed(utils.rpm2rad_per_sec(10)) 88 | dev.reset_all_pid() 89 | dev.move_to_pos(0) 90 | 91 | sleep(2) 92 | 93 | print("sine wave start !") 94 | # 目標位置からの偏差には以下のゲインを調整するか、振幅自体を変える 95 | # 振幅自体変えた方がベター?? 96 | 97 | # 新設 curveType:10 ダイレクト位置制御（速度制限、カーブなしでマスター側が時間管理して位置制御を行う。） 98 | dev.set_curve_type(10) 99 | dev.set_speed_p(3) # デフォルト 14 振動対策 100 | dev.set_speed_i(0) 101 | dev.set_pos_control_threshold(utils.deg2rad(1000)) 102 | dev.set_position_i(80) 103 | 104 | # # シグナルライブラリ（schedular関数を定期的に呼ぶ, timer_start(), timer_stop()も参照）で定期実行することの宣言 105 | signal.signal(signal.SIGALRM, scheduler) 106 | timer_start() 107 | 108 | try: 109 | while True: 110 | continue 111 | 112 | 113 | except KeyboardInterrupt: 114 | if dev: 115 | dev.disable_action() 116 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/usb-torque-control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import sys 10 | import os 11 | import pathlib 12 | from time import sleep 13 | 14 | current_dir = pathlib.Path(__file__).resolve().parent 15 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 16 | 17 | from pykeigan import usbcontroller 18 | from pykeigan import utils 19 | 20 | parser = argparse.ArgumentParser(description='モーター動作　トルク制御') 21 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 22 | args = parser.parse_args() 23 | 24 | os.system('clear') 25 | for i in range(6): 26 | print("　　　　　　　") 27 | print("\033[5;1H","---------------------------------------") 28 | sys.stdout.flush() 29 | 30 | """ 31 | ---------------------- 32 | モーター接続し、各種情報の取得 33 | ---------------------- 34 | """ 35 | torque=0 36 | position=0 37 | 38 | ##ログ情報callback 39 | def on_motor_log_cb(log): 40 | if log['error_codes']!='KM_SUCCESS': 41 | print('log {} '.format(log)) 42 | 43 | #接続 44 | dev=usbcontroller.USBController(args.port,False) 45 | dev.on_motor_log_cb=on_motor_log_cb 46 | 47 | """ 48 | ---------------------- 49 | モーター動作　トルク制御 50 | ---------------------- 51 | モーターを手で回して行くとトルクが加算され、重くなる。45度毎で0.025N*m増加 52 | """ 53 | torque_level=0 54 | 55 | ##トルクを監視し45度毎にトルクを増加 56 | def on_motor_measurement_cb(measurement): 57 | global torque_level 58 | torque=measurement['torque'] 59 | position=measurement['position'] 60 | now_torque_level=round(utils.rad2deg(position)/45)*0.025 61 | if torque_level!=now_torque_level: 62 | torque_level=now_torque_level 63 | dev.set_max_torque(abs(torque_level)) 64 | 65 | print('\033[4;1H\033[2K','torque/max_torque:{0:.2f}/{1:.2f}'.format(torque,torque_level)) 66 | sys.stdout.flush() 67 | 68 | def stop_torque_control_like_closing_cap(): 69 | if dev: 70 | dev.on_motor_measurement_value_cb=None 71 | dev.disable_action() 72 | dev.set_max_torque(10.0) 73 | 74 | def start_torque_control_like_closing_cap(): 75 | global torque_level 76 | print('\033[2;1H\033[2K', 'Please try to turn the motor by hand.') 77 | sys.stdout.flush() 78 | dev.disable_action() 79 | dev.preset_position(0) 80 | sleep(0.2) 81 | dev.enable_action() 82 | dev.move_to_pos(0,utils.rpm2rad_per_sec(10)) 83 | torque_level=10 84 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 85 | 86 | """ 87 | Exit with key input 88 | """ 89 | 90 | sleep(0.5) 91 | while True: 92 | print('\033[6;1H\033[2K') 93 | sys.stdout.flush() 94 | if sys.version_info<(3,0): 95 | inp = raw_input('Command input > Start:[s] Reset:[r] Exit:[Other key] >>') 96 | else: 97 | inp = input('Command input > Start:[s] Reset:[r] Exit:[Other key] >>') 98 | if inp == 's': 99 | start_torque_control_like_closing_cap() 100 | elif inp == 'r': 101 | stop_torque_control_like_closing_cap() 102 | elif inp !=None: 103 | print() 104 | dev.set_led(1, 100, 100, 100) 105 | dev.disable_action() 106 | dev.set_max_torque(10.0) 107 | sleep(0.2) 108 | dev.disconnect() 109 | break 110 | -------------------------------------------------------------------------------- /examples/windows_examples/sine_wave_move.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | import signal # タイマーで定期実行するためのシグナルライブラリ（高精度に定期実行できる） 16 | import math 17 | 18 | current_dir = pathlib.Path(__file__).resolve().parent 19 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 20 | 21 | from pykeigan import utils 22 | from pykeigan import usbcontroller 23 | 24 | RUN_CMD_INTERVAL = 0.01 25 | 26 | def select_port(): 27 | print('Available COM ports list') 28 | 29 | portlist = serial.tools.list_ports.comports() 30 | 31 | if not portlist: 32 | print('No available port') 33 | sys.exit() 34 | 35 | print('i : name') 36 | print('--------') 37 | for i, port in enumerate(portlist): 38 | print(i, ':', port.device) 39 | 40 | print('- Enter the port number (0~)') 41 | portnum = input() 42 | portnum = int(portnum) 43 | 44 | portdev = None 45 | if portnum in range(len(portlist)): 46 | portdev = portlist[portnum].device 47 | 48 | print('Connected to', portdev) 49 | 50 | return portdev 51 | 52 | 53 | 54 | # ログ情報callback 55 | def on_motor_log_cb(log): 56 | print('log {} '.format(log)) 57 | 58 | if log['error_codes'] == 'KM_SUCCESS': 59 | print('Command Success') 60 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 61 | print('Position Arrival Success') 62 | 63 | 64 | 65 | 66 | # モーター回転情報callback 67 | def on_motor_measurement_cb(measurement): 68 | print("\r"+'measurement {} '.format(measurement), end="") 69 | 70 | 71 | # 正弦波諸元 72 | f = 0.5 # 周波数 [s] 73 | ampDegree = 80 # 振幅 [degree] 74 | t = 0 75 | res = 0.01 # 時間分解能 [s] 76 | oneRound = f/res # 1周分 77 | 78 | # シグナルライブラリ（schedular関数を定期的に呼ぶ）のタイマースタート 79 | def timer_start(): 80 | signal.setitimer(signal.ITIMER_REAL, 1, res) # 第2引数の1は、1秒後にスタート、第3引数は、タイマー間隔 81 | 82 | # シグナルライブラリ（schedular関数を定期的に呼ぶ）のタイマーストップ 83 | def timer_stop(): 84 | signal.setitimer(signal.ITIMER_REAL, 0, res) # 第2引数を0にすると、タイマーは停止する決まり 85 | 86 | 87 | 88 | 89 | # シグナルライブラリで定期実行される関数。 90 | def scheduler(arg1, args2): 91 | global ampDegree, t, res, oneRound 92 | target = utils.deg2rad(ampDegree * math.sin(t * 2 * math.pi / oneRound)) 93 | dev.move_to_pos(target) 94 | t += res 95 | 96 | 97 | dev = usbcontroller.USBController(select_port()) 98 | #dev.on_motor_log_cb = on_motor_log_cb 99 | dev.on_motor_measurement_value_cb = on_motor_measurement_cb 100 | dev.enable_action() 101 | 102 | #dev.set_pos_control_threshold(utils.deg2rad(2)) 103 | #dev.set_position_p(10) 104 | #dev.set_position_i(2) 105 | dev.set_speed(utils.rpm2rad_per_sec(100)) 106 | # 連続で動作命令を送る場合、位置到達時の通知設定をOFFとする必要がある 107 | # dev.set_notify_pos_arrival_settings(False, 0.00872665, 200) # 第1引数 False で無効化 108 | dev.set_safe_run_settings(True, 100, 3) # 第1引数が True の場合、5000[ms]以内に次の動作命令が来ないと、停止する 0:free,1:disable,2:stop, 3:position固定 109 | dev.move_to_pos(0) 110 | 111 | sleep(3) 112 | dev.set_curve_type(10) # 10: ダイレクト位置制御（速度制限、カーブなしで位置制御を行う。振動対策） 113 | 114 | # シグナルライブラリ（schedular関数を定期的に呼ぶ, timer_start(), timer_stop()も参照）で定期実行することの宣言 115 | signal.signal(signal.SIGALRM, scheduler) 116 | timer_start() 117 | 118 | try: 119 | while True: 120 | continue 121 | 122 | 123 | except KeyboardInterrupt: 124 | if dev: 125 | dev.disable_action() 126 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/windows_examples/motor_measurement.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 18 | 19 | from pykeigan import usbcontroller 20 | 21 | 22 | def select_port(): 23 | print('Available COM ports list') 24 | 25 | portlist = serial.tools.list_ports.comports() 26 | 27 | if not portlist: 28 | print('No available port') 29 | sys.exit() 30 | 31 | print('i : name') 32 | print('--------') 33 | for i, port in enumerate(portlist): 34 | print(i, ':', port.device) 35 | 36 | print('- Enter the port number (0~)') 37 | portnum = input() 38 | portnum = int(portnum) 39 | 40 | portdev = None 41 | if portnum in range(len(portlist)): 42 | portdev = portlist[portnum].device 43 | 44 | print('Conncted to', portdev) 45 | 46 | return portdev 47 | 48 | 49 | for i in range(24): 50 | print("　　　　　　　") 51 | 52 | print("\033[19;2H","---------------------------------------", "\033[2;2H\033[2K", end="",flush=True) 53 | 54 | """ 55 | ---------------------- 56 | モーターに接続し、各種情報の取得 57 | ---------------------- 58 | """ 59 | ##モーター回転情報callback 60 | def on_motor_measurement_cb(measurement): 61 | print("\033[2;2H\033[2K", end="") 62 | print('measurement {} '.format(measurement), end="", flush=True) 63 | 64 | ##IMU情報callback 65 | def on_motor_imu_measurement_cb(measurement): 66 | print("\033[6;2H\033[2K", end="") 67 | print('imu_measurement {} '.format(measurement), end="", flush=True) 68 | 69 | ##ログ情報callback 70 | def on_motor_log_cb(log): 71 | print("\033[12;2H\033[2K", end="", flush=True) 72 | print('log {} '.format(log), end="", flush=True) 73 | 74 | ##エラー情報callback 75 | def on_motor_connection_error_cb(e): 76 | print("\033[16;2H\033[2K", end="", flush=True) 77 | print('error {} '.format(e), end="", flush=True) 78 | 79 | 80 | #接続 81 | #dev=usbcontroller.USBController('/dev/ttyUSB0',False)#モーターのアドレス 参照 usb-simple-connection.py 82 | dev=usbcontroller.USBController(select_port()) #モーターのアドレス 参照 usb-simple-connection.py 83 | dev.on_motor_measurement_value_cb=on_motor_measurement_cb 84 | dev.on_motor_imu_measurement_cb=on_motor_imu_measurement_cb 85 | dev.on_motor_log_cb=on_motor_log_cb 86 | dev.on_motor_connection_error_cb=on_motor_connection_error_cb 87 | 88 | dev.enable_continual_imu_measurement()#IMUはデフォルトでOFFの為、取得する場合Onにする 89 | 90 | # ビットフラグ 0x40 でモーターの時刻送信を有効化 ※ モーターFW ver 2.62以降対応、ver2.61以下は無効 91 | dev.set_motor_measurement_settings(5) 92 | 93 | #モーター動作 94 | dev.set_led(2,255,255,0) 95 | sleep(3) 96 | dev.enable_action() 97 | dev.set_speed(1.0) 98 | dev.run_forward() 99 | sleep(10) 100 | dev.disable_action() 101 | 102 | 103 | """ 104 | Exit with key input 105 | """ 106 | 107 | sleep(0.5) 108 | 109 | try: 110 | while True: 111 | print("\033[20;2H", end="",flush=True) 112 | print("---------------------------------------") 113 | inp = input('Exit:[key input] >>') 114 | if inp !=None: 115 | dev.set_led(1, 100, 100, 100) 116 | dev.disable_action() 117 | dev.disconnect() 118 | break 119 | 120 | 121 | except KeyboardInterrupt: 122 | if dev: 123 | dev.disable_action() 124 | dev.disconnect() 125 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/usb-position-control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import signal 10 | import sys 11 | import os 12 | import pathlib 13 | from time import sleep 14 | 15 | from concurrent.futures import ThreadPoolExecutor 16 | 17 | current_dir = pathlib.Path(__file__).resolve().parent 18 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 19 | 20 | from pykeigan import usbcontroller 21 | from pykeigan import utils 22 | 23 | parser = argparse.ArgumentParser(description="モーター動作 相対移動と絶対位置移動") 24 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 25 | args = parser.parse_args() 26 | 27 | os.system('clear') 28 | for i in range(10): 29 | print("　　　　　　　") 30 | print("\033[9;2H","---------------------------------------", "\033[2;2H\033[2K") 31 | sys.stdout.flush() 32 | 33 | """ 34 | ---------------------- 35 | モーターに接続し、各種情報の取得 36 | ---------------------- 37 | """ 38 | ##モーター回転情報callback 39 | def on_motor_measurement_cb(measurement): 40 | print("\033[2;2H\033[2K") 41 | print('measurement {} '.format(measurement)) 42 | sys.stdout.flush() 43 | 44 | ##ログ情報callback 45 | def on_motor_log_cb(log): 46 | print("\033[5;2H\033[2K") 47 | sys.stdout.flush() 48 | print('log {} '.format(log)) 49 | sys.stdout.flush() 50 | 51 | #接続 52 | dev=usbcontroller.USBController('/dev/ttyUSB0',False) 53 | dev.on_motor_measurement_value_cb=on_motor_measurement_cb 54 | dev.on_motor_log_cb=on_motor_log_cb 55 | 56 | """ 57 | ---------------------- 58 | モーター動作 相対移動 59 | ---------------------- 60 | """ 61 | dev.set_led(2,255,255,0) 62 | sleep(3) 63 | dev.enable_action() 64 | dev.set_speed(utils.rpm2rad_per_sec(10))#rpm-> rad/sec 65 | 66 | dev.move_by_dist(utils.deg2rad(180),None)#Degree-> rad 67 | sleep(5) 68 | dev.move_by_dist(utils.deg2rad(-180),None) 69 | sleep(5) 70 | dev.move_by_dist(utils.deg2rad(360),utils.rpm2rad_per_sec(15))#rpm-> rad/sec 71 | sleep(6) 72 | 73 | """ 74 | ---------------------- 75 | モーター動作 絶対位置移動 76 | ---------------------- 77 | """ 78 | dev.set_curve_type(1) 79 | dev.set_led(2,0,255,255) 80 | dev.set_speed(utils.rpm2rad_per_sec(30)) 81 | dev.preset_position(0)#現在位置の座標を0に設定 82 | dev.move_to_pos(utils.deg2rad(90),(utils.deg2rad(90)/3)) 83 | sleep(4) 84 | dev.move_to_pos(utils.deg2rad(180),(utils.deg2rad(90)/3)) 85 | sleep(4) 86 | dev.move_to_pos(utils.deg2rad(360),(utils.deg2rad(180)/3)) 87 | sleep(4) 88 | dev.move_to_pos(utils.deg2rad(720),(utils.deg2rad(360)/3)) 89 | sleep(4) 90 | dev.move_to_pos(utils.deg2rad(0),(utils.deg2rad(720)/4)) 91 | sleep(5) 92 | dev.set_led(2, 255, 50, 255) 93 | dev.set_curve_type(0)#Turn off Motion control 94 | dev.move_to_pos(utils.deg2rad(90),(utils.deg2rad(90)/0.5)) 95 | sleep(2) 96 | dev.move_to_pos(utils.deg2rad(180),(utils.deg2rad(90)/0.5)) 97 | sleep(2) 98 | dev.move_to_pos(utils.deg2rad(90),(utils.deg2rad(90)/0.5)) 99 | sleep(2) 100 | dev.move_to_pos(utils.deg2rad(360),(utils.deg2rad(270)/1)) 101 | sleep(2) 102 | 103 | dev.set_led(1,255,255,0) 104 | dev.disable_action() 105 | 106 | """ 107 | Exit with key input 108 | """ 109 | 110 | sleep(0.5) 111 | while True: 112 | print("\033[10;2H") 113 | sys.stdout.flush() 114 | print("---------------------------------------") 115 | if sys.version_info <(3,0): 116 | inp = raw_input('Exit:[key input] >>') 117 | else: 118 | inp = input('Exit:[key input] >>') 119 | if inp !=None: 120 | dev.set_led(1, 100, 100, 100) 121 | dev.set_curve_type(1) 122 | dev.disable_action() 123 | dev.disconnect() 124 | break 125 | -------------------------------------------------------------------------------- /examples/windows_examples/pid_settings.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: Takashi Tokuda 6 | Keigan Inc. 7 | """ 8 | 9 | import argparse 10 | import sys 11 | import pathlib 12 | import msvcrt 13 | import serial.tools.list_ports 14 | from time import sleep 15 | 16 | current_dir = pathlib.Path(__file__).resolve().parent 17 | sys.path.insert(0, str(current_dir) + '/../../') # give priority to the directory where pykeigan is 18 | 19 | from pykeigan import utils 20 | from pykeigan import usbcontroller 21 | 22 | def select_port(): 23 | print('Available COM ports list') 24 | 25 | portlist = serial.tools.list_ports.comports() 26 | 27 | if not portlist: 28 | print('No available port') 29 | sys.exit() 30 | 31 | print('i : name') 32 | print('--------') 33 | for i, port in enumerate(portlist): 34 | print(i, ':', port.device) 35 | 36 | print('- Enter the port number (0~)') 37 | portnum = input() 38 | portnum = int(portnum) 39 | 40 | portdev = None 41 | if portnum in range(len(portlist)): 42 | portdev = portlist[portnum].device 43 | 44 | print('Connected to', portdev) 45 | 46 | return portdev 47 | 48 | 49 | 50 | # ログ情報callback 51 | def on_motor_log_cb(log): 52 | print('log {} '.format(log)) 53 | 54 | if log['error_codes'] == 'KM_SUCCESS': 55 | print('Command Success') 56 | elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL': 57 | print('Position Arrival Success') 58 | 59 | 60 | 61 | 62 | # モーター回転情報callback 63 | def on_motor_measurement_cb(measurement): 64 | print("\r"+'measurement {} '.format(measurement), end="") 65 | 66 | 67 | def read_pid_settings(motor): 68 | print('\rread_pid_settings') 69 | qCurrentP = motor.read_qcurrent_p() 70 | qCurrentI = motor.read_qcurrent_i() 71 | qCurrentD = motor.read_qcurrent_d() 72 | speedP = motor.read_speed_p() 73 | speedI = motor.read_speed_i() 74 | speedD = motor.read_speed_d() 75 | positionP = motor.read_position_p() 76 | positionI = motor.read_position_i() 77 | positionD = motor.read_position_d() 78 | threshold = motor.read_pos_control_threshold() 79 | print('-------') 80 | print('qCurrent gain P: ',qCurrentP) 81 | print('qCurrent gain I: ',qCurrentI) 82 | print('qCurrent gain D: ',qCurrentD) 83 | print('speed gain P: ',speedP) 84 | print('speed gain I: ',speedI) 85 | print('speed gain D: ',speedD) 86 | print('position gain P: ',positionP) 87 | print('position gain I: ',positionI) 88 | print('position gain D: ',positionD) 89 | print('position PID threshold [rad]: ', threshold) 90 | print('position PID threshold [deg]: ', utils.rad2deg(threshold)) 91 | print('-------') 92 | 93 | def read_device_name(motor): 94 | name = motor.read_device_name() 95 | print('Device Name: ', name) 96 | 97 | 98 | if __name__ == '__main__': 99 | dev = usbcontroller.USBController(select_port()) 100 | read_device_name(dev) 101 | read_pid_settings(dev) 102 | try: 103 | while True: 104 | sleep(0.01) 105 | if msvcrt.kbhit(): 106 | c = msvcrt.getwch() 107 | print(c) 108 | 109 | if c == 'r': 110 | read_pid_settings(dev) 111 | elif c == 'n': 112 | read_device_name(dev) 113 | elif c == 's': 114 | dev.save_all_registers() 115 | elif c == 'd': 116 | dev.reset_all_pid() 117 | elif c == 'b': 118 | dev.reboot() 119 | 120 | except KeyboardInterrupt: 121 | if dev: 122 | dev.disable_action() 123 | print('Ctrl-C') -------------------------------------------------------------------------------- /examples/usb-teaching-control.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thr Jan 10 09:13:24 2018 4 | 5 | @author: takata@innovotion.co.jp 6 | @author: harada@keigan.co.jp 7 | """ 8 | import argparse 9 | import signal 10 | import sys 11 | import os 12 | import math 13 | import pathlib 14 | from time import sleep 15 | 16 | from concurrent.futures import ThreadPoolExecutor 17 | 18 | current_dir = pathlib.Path(__file__).resolve().parent 19 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 20 | 21 | from pykeigan import usbcontroller 22 | from pykeigan import utils 23 | 24 | parser = argparse.ArgumentParser(description='モーター動作　ティーチング記録・再生') 25 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 26 | args = parser.parse_args() 27 | 28 | REC_NUMBER=1 29 | 30 | """ 31 | ---------------------- 32 | モーター接続し、各種情報の取得 33 | ---------------------- 34 | """ 35 | 36 | ##ログ情報callback 37 | def on_motor_log_cb(log): 38 | if log['error_codes']!='KM_SUCCESS': 39 | print('log {} '.format(log)) 40 | 41 | #接続 42 | dev=usbcontroller.USBController(args.port,False) 43 | dev.on_motor_log_cb=on_motor_log_cb 44 | 45 | """ 46 | ---------------------- 47 | モーター動作　ティーチング記録・再生 48 | ---------------------- 49 | """ 50 | def rec_and_play_teaching(index): 51 | dev.disable_action() 52 | dev.stop_teaching_motion() 53 | dev.erase_motion(index)#消してないとエラー無しで録画出来ない 54 | sleep(1)#erase_motionの実行完了までのインターバルが必要。約500ms 55 | dev.set_led(2, 255, 255, 0) 56 | dev.start_teaching_motion(index,5000) # インデックス,記録時間 57 | print("") 58 | for i in range(5): 59 | print("\033[1G\033[2K","モーターを動かして下さい。 残り:",5-i," sec") 60 | sys.stdout.flush() 61 | sleep(1) 62 | dev.stop_teaching_motion() 63 | print("") 64 | dev.set_led(1, 100, 100, 100) 65 | sleep(2) 66 | play_teaching(index) 67 | return True 68 | 69 | def play_teaching(index): 70 | print("\033[1G\033[2K","ティーチング再生中") 71 | sys.stdout.flush() 72 | dev.set_led(2, 0, 255, 255) 73 | dev.stop_playback_motion() 74 | dev.enable_action() 75 | dev.start_playback_motion(index,1,1) 76 | sleep(5) 77 | print("\033[1G\033[2K", "ティーチング完了") 78 | dev.set_led(1, 100, 100, 100) 79 | return True 80 | 81 | def read_comp_cb(index,motion_value): 82 | print("read motion data "+str(index)+" >>>>>>>>>>>>>>") 83 | print(motion_value) 84 | print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>") 85 | 86 | def read_motion_exec(index): 87 | dev.read_motion(index,read_comp_cb) 88 | 89 | def write_motion_position_exec(index): 90 | print("Write Test motion>>>>" ) 91 | dev.enable_action() 92 | dev.erase_motion(index) 93 | sleep(2) 94 | dev.prepare_teaching_motion(index,0) 95 | sleep(1) 96 | amp = math.pi/2 #deg 97 | len=500 98 | for i in range(len): 99 | pos=amp*math.cos(2*math.pi*i/500) 100 | dev.write_motion_position(pos) 101 | sleep(0.02) 102 | print('\r Write Test motion>>>>{0} {1}/{2}'.format(pos,i,len)) 103 | pass 104 | sleep(0.5) 105 | dev.stop_teaching_motion() 106 | sleep(1) 107 | print("") 108 | print("Write Test motion>>>>comp") 109 | play_teaching(REC_NUMBER) 110 | 111 | """ 112 | Exit with key input 113 | """ 114 | 115 | 116 | sleep(0.5) 117 | while True: 118 | print("---------------------------------------") 119 | if sys.version_info<(3,0): 120 | inp = raw_input('Command input > Rec:[r] Replay:[p] Write Test motion:[w] Read motion:[m] Exit:[Other key] >>') 121 | else: 122 | inp = input('Command input > Rec:[r] Replay:[p] Write Test motion:[w] Read motion:[m] Exit:[Other key] >>') 123 | if inp == 'r': 124 | rec_and_play_teaching(REC_NUMBER) 125 | elif inp == 'p': 126 | play_teaching(REC_NUMBER) 127 | elif inp == 'w': 128 | write_motion_position_exec(REC_NUMBER) 129 | elif inp == 'm': 130 | read_motion_exec(REC_NUMBER) 131 | elif inp !=None: 132 | dev.set_led(1, 100, 100, 100) 133 | dev.disable_action() 134 | dev.disconnect() 135 | break 136 | -------------------------------------------------------------------------------- /tools/log/sample.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import time 3 | 4 | import log 5 | 6 | import argparse 7 | import sys 8 | import pathlib 9 | from time import sleep 10 | 11 | 12 | current_dir = pathlib.Path(__file__).resolve().parent 13 | sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists 14 | 15 | from pykeigan import usbcontroller 16 | 17 | parser = argparse.ArgumentParser(description='モーターに接続し、各種情報の取得') 18 | parser.add_argument('port',metavar='PORT',default='/dev/ttyUSB0',nargs='?',help='モーターのデバイスファイル指定 (default:/dev/ttyUSB0)') 19 | args = parser.parse_args() 20 | 21 | # ----------------------------------------------- User callback / ユーザーコールバック ------------------------------------------------------------------------------ 22 | # In case any of the below callback is used in main program, ALL needed callback log has to be added manually (please call additionalLog(msg) function) 23 | # メインプログラムが以下のいずれかのコールバック使う場合、コールバックからの必要なログを全てのマニュアルで追加必要 （additionalLog(msg)関数使ってください） 24 | def on_motor_log_cb(log): 25 | print('on_motor_log_cb used in main program') 26 | print('\33[34m', '[Main] log {} '.format(log), '\33[0m') 27 | # >>>>>> add additional message to logging / ログに追加メーセージ書き出し >>>>>> 28 | motor_log.additionalLog("log: "+str(log)) 29 | 30 | def on_motor_connection_error_cb(log): 31 | print('on_motor_connection_error_cb used in main program') 32 | print('\33[34m', '[Main] connection error {} '.format(log), '\33[0m') 33 | # >>>>>> add additional message to logging / ログに追加メーセージ書き出し >>>>>> 34 | motor_log.additionalLog("connection error: "+str(log)) 35 | 36 | def on_motor_reconnection_cb(log): 37 | print('on_motor_reconnection_cb used in main program') 38 | print('\33[34m', '[Main] reconnection {} '.format(log), '\33[0m') 39 | # >>>>>> add additional message to logging / ログに追加メーセージ書き出し >>>>>> 40 | motor_log.additionalLog("reconnection: "+str(log)) 41 | 42 | # ----------------------------------------------------------------------------------------------------------------------------- 43 | 44 | #接続 45 | dev=usbcontroller.USBController(args.port,False) 46 | 47 | # >>>>>> motor log >>>>>> 48 | # Please initiate MotorLog after setting all initiate parameter of motor (dev). 49 | # モーター（dev）の最初設定の後に MotorLogを作成してください 50 | motor_log = log.MotorLog(dev) 51 | 52 | # >>>>>> motor log (callback is used in main program / メインプログラムがコールバック使う) >>>>>> 53 | # User callback / ユーザーコールバック 54 | #dev.on_motor_log_cb=on_motor_log_cb 55 | #dev.on_motor_connection_error_cb=on_motor_connection_error_cb 56 | #dev.on_motor_reconnection_cb=on_motor_reconnection_cb 57 | # Please initiate MotorLog after setting all initiate parameter of motor (dev). 58 | # モーター（dev）の最初設定の後に MotorLogを作成してください 59 | #motor_log = log.MotorLog(dev, 'Not_overwrite_cb', 1, False) 60 | 61 | # ---------------------------------------------- User function / ユーザー関数 ------------------------------------------------------------------------------- 62 | def motorMove(): 63 | try: 64 | print('\33[34m', "[Motor] move call", '\33[0m') 65 | dev.set_led(2, 255, 255, 0) 66 | sleep(3) 67 | # >>>>>> pause logging / ログ一時停止 >>>>>> 68 | motor_log.pauseLogging() 69 | sleep(2) 70 | # >>>>>> resume logging / ログ再開 >>>>>> 71 | motor_log.resumeLogging() 72 | 73 | # >>>>>> add additional message to logging / ログに追加メーセージ書き出し >>>>>> 74 | motor_log.additionalLog("Forward") 75 | print('\33[34m', "[Motor] start moving forward", '\33[0m') 76 | dev.enable_action() 77 | dev.set_speed(1.0) 78 | dev.run_forward() 79 | sleep(10) 80 | 81 | # >>>>>> add additional message to logging / ログに追加メーセージ書き出し >>>>>> 82 | motor_log.additionalLog("Backward") 83 | print('\33[34m', "[Motor] moving backward", '\33[0m') 84 | dev.run_reverse() 85 | sleep(10) 86 | 87 | dev.disable_action() 88 | print('\33[34m', "[Motor] end", '\33[0m') 89 | time.sleep(2) 90 | 91 | except Exception: 92 | print('\33[34m', "[Motor] end by interrupt", '\33[0m') 93 | sys.exit(-1) 94 | finally: 95 | # >>>>>> stop logging / ログ終了 >>>>>> 96 | motor_log.stopLogging() 97 | # ----------------------------------------------------------------------------------------------------------------------------- # 98 | 99 | # >>>>>> start logging motor data / モーターデータログ開始 >>>>>> 100 | motor_log.startLogging() 101 | 102 | # start motor control (User function) / モーターデータ操作開始（ユーザーの関数） 103 | motorMove() 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /examples/windows_examples/keigan_motor_measurement_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # coding:utf-8 3 | import argparse 4 | import sys 5 | import pathlib 6 | import msvcrt 7 | import serial.tools.list_ports 8 | from time import sleep 9 | import time 10 | 11 | current_dir = pathlib.Path(__file__).resolve().parent 12 | sys.path.insert(0, str(current_dir) + '/../../') # give 1st priority to the directory where pykeigan exists 13 | 14 | from pykeigan import utils 15 | from pykeigan import usbcontroller 16 | 17 | # '0: 115200' 18 | # '1: 230400' 19 | # '2: 250000' 20 | # '3: 460800' 21 | # '4: 921600' 22 | # '5: 1000000 (1M)' 23 | 24 | baud_list = [115200, 230400, 250000, 460800, 921600, 1000000] 25 | 26 | def current_baud_rate(): 27 | print('Select current baud rate (bps) of KeiganMotor side') 28 | print("0: 115200, 1: 230400, 2: 250000, 3: 460800, 4: 921600, 5: 1000000 (1M)") 29 | print("Default is 0: 115200") 30 | print('- Enter the port number (0~5)') 31 | 32 | baud_num = int(input()) 33 | 34 | if baud_num > 5 or baud_num < 0: 35 | baud_num = 0 36 | 37 | baud_rate = baud_list[baud_num] 38 | 39 | return baud_rate 40 | 41 | 42 | def select_port(): 43 | print('Available COM ports list') 44 | 45 | portlist = serial.tools.list_ports.comports() 46 | 47 | if not portlist: 48 | print('No available port') 49 | sys.exit() 50 | 51 | print('i : name') 52 | print('--------') 53 | for i, port in enumerate(portlist): 54 | print(i, ':', port.device) 55 | 56 | print('- Enter the port number (0~)') 57 | portnum = input() 58 | portnum = int(portnum) 59 | 60 | portdev = None 61 | if portnum in range(len(portlist)): 62 | portdev = portlist[portnum].device 63 | 64 | print('Connected to', portdev) 65 | 66 | return portdev 67 | 68 | 69 | 70 | class KeiganMotor(): 71 | def __init__(self): 72 | # measurement_method: コールバック関数を設定する場合の観測方法 73 | measurement_method = "callback" 74 | 75 | # measurement_method: read_motor_measurement関数を使用する場合の観測方法 76 | # measurement_method = "read" 77 | 78 | self.dev=usbcontroller.USBController(select_port(), baud=current_baud_rate()) 79 | self.motor_measurement_start = time.perf_counter() 80 | 81 | self.dev.preset_position(0) 82 | self.motor_measurement_count = 0 83 | self.before_motor_angle = 0 84 | 85 | # ビットフラグ 0x40 でモーターの時刻送信を有効化 ※ モーターFW ver 2.62以降対応 86 | self.dev.set_motor_measurement_settings(5) 87 | 88 | self.dev.set_curve_type(0) 89 | self.dev.set_speed_i(0) 90 | self.dev.enable_action() 91 | self.dev.run_at_velocity(1) 92 | 93 | # コールバックを使う（自動通知：有効） 94 | if measurement_method == "callback": 95 | print("callback registered") 96 | self.dev.on_motor_measurement_value_cb = self.on_motor_measurement_cb 97 | # コールバックを使わず、プログラムから読み出す（自動通知：無効） 98 | elif measurement_method == "read": 99 | self.dev.disable_continual_motor_measurement() 100 | while True: 101 | try: 102 | if measurement_method == "read": 103 | self.motor_measurement() 104 | time.sleep(0.002) 105 | except KeyboardInterrupt: 106 | print("Break") 107 | break 108 | 109 | time.sleep(100) # 100秒待つ 110 | 111 | 112 | 113 | def motor_measurement(self): 114 | # 観測値が更新されるまでの時間を計測 115 | measurement = self.dev.read_motor_measurement() 116 | self.angle = measurement["position"] 117 | print(self.motor_measurement_count, time.perf_counter(), measurement["motor_time"], self.angle) 118 | if self.angle != self.before_motor_angle: 119 | measurement_time = time.perf_counter() - self.motor_measurement_start 120 | print('measurement_time: {}'.format(measurement_time)) 121 | print("angle: {}".format(self.angle)) 122 | self.motor_measurement_start = time.perf_counter() 123 | self.before_motor_angle = self.angle 124 | 125 | def on_motor_measurement_cb(self, measurement): 126 | # 100回分の観測周期の平均値の計測 127 | self.angle = measurement["position"] 128 | # print(measurement) 129 | print(self.motor_measurement_count, time.perf_counter(), measurement["motor_time"], self.angle) 130 | # if self.motor_measurement_count == 100: 131 | # measure_time = time.time() - self.motor_measurement_start 132 | # print("measurement_time: {}".format(measure_time/100)) 133 | # self.motor_measurement_start = time.time() 134 | # self.motor_measurement_count=0 135 | self.motor_measurement_count=self.motor_measurement_count+1 136 | 137 | if __name__ == '__main__': 138 | km = KeiganMotor() 139 | -------------------------------------------------------------------------------- /tools/log/readme.md: -------------------------------------------------------------------------------- 1 | # Read me 2 | 3 | ## Description / 説明 4 | Program for logging motor data (external) 5 | モーターのログ書き出しプログラム 6 | 7 | ### File / ファイル 8 | 1. log.py 9 | main function / メイン関数 10 | 2. sample.py 11 | sample code of how to use / 利用例のコード 12 | 13 | ### How to Use / 使い方 14 | 1. Importing `log.py` / `log.py`をインポートする 15 | ``` 16 | import log 17 | ``` 18 | 2. Creating a MotorLog / MotorLogの作成 19 | モータ(dev)の起動パラメータを全て設定した後に行ってください。 20 | 21 | ``` 22 | motor_log = log.MotorLog(dev) 23 | ``` 24 | 3. start logging / ログ開始 25 | ``` 26 | motor_log.startLogging() 27 | ``` 28 | 4. to finish logging, please use the command below 29 | ログ終了したい場合は以下のコマンドを使用します。 30 | ``` 31 | motor_log.stopLogging() 32 | ``` 33 | 5. to pause and resume logging, please use the command below (respectively) 34 | ロギングを一時停止、再開するには、以下のコマンドを使用します。 35 | ``` 36 | motor_log.pauseLogging() 37 | motor_log.resumeLogging() 38 | ``` 39 | 40 | 6. To add additional message to log file, please use the command below 41 | ログファイルにメッセージを追加するには、以下のコマンドを使用してください。 42 | ``` 43 | motor_log.additionalLog('addtional message') 44 | ``` 45 | 46 | For a sample of how to use, please refer to `sample.py` 47 | 利用例のコードは`sample.py`をご参考ください。 48 | 49 | ### Note / ノート 50 | #### Parameter / パラメータ 51 | > MotorLog(*motor*, *file_name(optional)*, *interval_second(optional)*, *overwriteCb(optional)*) 52 | 53 | | parameter/パラメータ | type/タイプ | default value / デフォルト値 | explanation/説明 | 54 | |-----------------|---------------|------------------------|-------------------------------------------------------| 55 | | motor | USBController | - | motor to log / ログ書き出しのモーター | 56 | | file_name | string | port name | file name of log / ログファイル名 | 57 | | interval | number | 1 | interval between getting data (sec) / データを取得する間隔（秒） | 58 | | overwriteCb | boolean | True | overwrite callback to emit log / コールバックにログを書き出すように上書き | 59 | 60 | 61 | sample / 例 : `MotorLog(dev, 'Test_Motor', 2)` 62 | 63 | #### File name / ファイル名 64 | `file_name.log.YYYY-mm-dd_HH-MM-SS` 65 | sample / 例 : `Test_Motor.log.2023-01-11_08-55-22` 66 | 67 | #### Log format / ログフォーマット 68 | `Timestamp | {pos, vel, toq, unix, motor}, {enable, queue, ctrl}` 69 | 70 | | parameter/パラメータ | explanation/説明 | 71 | |-----------------|--------------------------------------| 72 | | Timestamp | current datetime / 現在 datetime | 73 | | pos | position (3 decimal digit / 小数第3位) | 74 | | vel | velocity (3 decimal digit / 小数第3位) 　 | 75 | | toq | toque　(3 decimal digit / 小数第3位) 　 | 76 | | unix | received_unix_time | 77 | | motor | motor_time | 78 | | enable | motorEnabled | 79 | | queue | queue | 80 | | ctrl | motor_control_mode | 81 | 82 | sample / 例 : 83 | `2023-01-12 10:39:42.046269 | {pos:41.827,vel:0.226,toq:0.038,unix:1673487582.003,motor:3541267}{enable:1,queue:0,ctrl:MOTOR_CONTROL_MODE_VELOCITY}` 84 | 85 | additional message will be logged with *[add]* tag. 86 | 追加メッセージは、*[add]*タグで記録されます。 87 | 88 | sample / 例 : 89 | `2023-01-12 10:39:40.917516[add] | {'command_names': 96, 'error_codes': 0}` 90 | 91 | #### callback log / コールバックのログ 92 | | Callback/コールバック | tag/タグ | format/フォーマット | sample/例 | 93 | |-----------------------------------|-------------|----------------------------|-------------------------------------------| 94 | | on_motor_log_cb | log | command_names, error_codes | `{'command_names': 96, 'error_codes': 0}` | 95 | | on_motor_reconnection_cb | reconnect | reconn_err_cnt | `4` | 96 | | on_motor_connection_error_cb | connect err | error message | `[Errno 5] Input/output error` | 97 | 98 | **warning** 99 | In case any of the listed callback is used in main program, ALL needed callback log has to be added manually 100 | (please call `additionalLog(msg)` function) 101 | please refer to `sample.py` for a sample code 102 | 103 | **注意** 104 | リストされたコールバックのいずれかがメインプログラムで使用される場合、他の必要なログもコールバックに手動で追加する必要があります。 105 | （`additionalLog(msg)` 関数使ってください） 106 | サンプルコードについては `sample.py` を参照してください。 107 | 108 | 109 | 110 | #### Log Rotation / ログローテーション 111 | Log file will be automatically rotated every 4 hours. 112 | The last log before rotating will be saved as `file_name.log`. 113 | In case logging is ended properly (`stopLogging()` is called), the last file will be automatically created (`file_name.log.YYYYmmdd-HHMMSS` format). 114 | 115 | **warning** 116 | - if logging is not ended properly, please manually back up the last log file, otherwise it might be replaced with the next running 117 | - The number of backup file for each *file_name* is up to 120 files (if exceeded, the oldest file will be deleted). 118 | 119 | 4時間毎に自動的にログファイルローテーション行います。 120 | ローテーション前の最後のログは`file_name.log`として保存されます。 121 | ログプログラムが適切に終了した（`stopLogging()`呼ばれた）場合は、最後のログは自動でバックアップされます（`file_name.log.YYYYmmdd-HHMMSS`フォーマット）。 122 | 123 | **注意** 124 | - ログプログラムが適切に終了していない場合は、最後のログファイルを手動でバックアップしてください。次のプログラム実行に以前のプログラムのログが上書きされる可能性があります。 125 | - 各*file_name*のログは120ファイルまで。（120ファイルを超えた場合は最古のログから削除されます） -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | Python Library for Keigan Motor (v2) 2 | ============================================== 3 | 4 | Introduction 5 | --------------- 6 | You can control your Keigan Motor through USB Serial and BLE. 7 | 8 | https://www.keigan-motor.com/ 9 | 10 | **This library has been updated from v1 to more than v2.** 11 | 12 | **The method names are not comatible, but we added many important features.** 13 | 14 | **We strongly recommend more than v2 from now on.** 15 | 16 | **Just in case you can get v1 from: https://github.com/keigan-motor/pykeigan_motor/tree/v1** 17 | 18 | At present we support Linux only for BLE, because the BLE functions of this library depends on bluepy(Python interface to Bluetooth LE on Linux by Mr. Ian Harvey): 19 | 20 | https://github.com/IanHarvey/bluepy 21 | 22 | The USB serial functions should work on Windows and Mac too. 23 | 24 | Requirements 25 | ------------------ 26 | 27 | - python >= 3.5 (recommended) 28 | - pyserial >= 3.4 29 | - bluepy >= 1.1.4 (BLE support. Linux only) 30 | 31 | **NOTE) This library runs on python 3.5 or later.** 32 | 33 | **If you want to use it with python 2.7 in ROS, etc., please use 2.2.0.** 34 | 35 | **https://github.com/keigan-motor/pykeigan_motor/tree/2.2.0** 36 | 37 | **We recommend to update your KeiganMotor firmware to the latest as python script can cause error if it use new API commands.** 38 | 39 | 40 | *Update 2.4.0* 41 | 42 | - support both python2.7 and 3 43 | 44 | 45 | 46 | Installation 47 | ------------------------------- 48 | 49 | Install from source:: 50 | 51 | sudo apt install git 52 | git clone https://github.com/keigan-motor/pykeigan_motor 53 | cd pykeigan_motor 54 | python setup.py install 55 | # and to install BLE Support 56 | sudo apt-get install python-pip libglib2.0-dev 57 | sudo pip install bluepy 58 | pip install .[ble] 59 | 60 | Install from PyPI:: 61 | 62 | pip3 install pykeigan-motor 63 | # or to install BLE Support 64 | pip3 install pykeigan-motor[ble] 65 | 66 | - NOTE) Use pip instead of pip3 in case of Python2.x or Windows OS. 67 | 68 | USB Serial 69 | ----------------- 70 | To connect your Keigan Motor through USB serial, you need to know the mounted path. 71 | You can get the unique path of your Keigan Motor by:: 72 | 73 | ls /dev/serial/by-id/ 74 | 75 | Your Keigan Motor's ID should be like 'usb-FTDI_FT230X_Basic_UART_DM00XXXX-if00-port0'. 76 | To use your Keigan Motor through USB serial, you need to add R/W permission to it.:: 77 | 78 | sudo chmod 666 /dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DM00XXXX-if00-port0 79 | 80 | - Simplest Sample Code 81 | Rotate counter-clockwise with 1.0 rad/sec. 82 | 83 | .. code-block:: python 84 | 85 | from pykeigan import usbcontroller 86 | dev=usbcontroller.USBContoller('/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DM00xxxx-if00-port0') 87 | dev.enable_action() 88 | dev.set_speed(1.0) 89 | dev.run_forward() 90 | 91 | - examples/usb-simple-connection.py 92 | Basic connection to the Motor. 93 | - examples/usb-rotate-the-motor.py 94 | Rotate the Motor continuously and stop. 95 | - examples/usb-position-control.py 96 | Rotate the Motor to the relative and absolute position. 97 | - examples/usb-get-motor-Informations.py 98 | Acquire the speed, position, torque and IMU values of the Motor. 99 | - examples/usb-actuator.py 100 | Let the Motor go and return for the specific distance. 101 | - examples/usb-torque-control.py 102 | Demonstration for a torque control. Increase the torque as you rotate the Motor by hand. 103 | - examples/usb-teaching-control.py 104 | Let the Motor record and playback your motion. 105 | 106 | BLE (for Linux Only) 107 | ---------------------- 108 | You need to know the MAC address of your Keigan Motor for BLE connection. 109 | 110 | For example, you can use the following simple script. Please run with sudo. 111 | 112 | KM1Scan.py 113 | 114 | .. code-block:: python 115 | 116 | from bluepy.btle import Scanner 117 | scanner=Scanner() 118 | devices=scanner.scan(5.0) 119 | for dev in devices: 120 | for (adtype, desc, value) in dev.getScanData(): 121 | if desc=="Complete Local Name" and "KM-1" in value: 122 | print(value,":",dev.addr) 123 | 124 | - Simplest Sample Code 125 | Rotate counter-clockwise with 1.0 rad/sec. 126 | 127 | .. code-block:: python 128 | 129 | from pykeigan import blecontroller 130 | dev=blecontroller.BLEController("xx:xx:xx:xx:xx") 131 | dev.enable_action() 132 | dev.set_speed(1.0) 133 | dev.run_forward() 134 | 135 | - examples/ble-simple-connection.py 136 | Basic connection to the Motor. 137 | - examples/ble-scanner-connection.py 138 | Connect to the Motor by BLE scanning. 139 | - examples/ble-rotate-the-motor.py 140 | Rotate the Motor continuously and stop. 141 | - examples/ble-get-motor-Informations.py 142 | Acquire the speed, position, torque and IMU values of the Motor. 143 | 144 | Release Notes 145 | ------------------ 146 | Release 2.4.0 147 | 148 | - Added support both python2 and 3 support 149 | - Update some examples 150 | - Add reinit, reconnection during initialize 151 | 152 | Release 2.2.5 153 | 154 | - Respond to KeiganMotor firmware ver >= 2.40 155 | - Bug fixes 156 | - Added serial reconnection feature(USB) 157 | - Added some new APIs 158 | - Added some examples 159 | 160 | Release 2.1.0 161 | 162 | - Added python 2 support 163 | 164 | Release 2.0.1 165 | 166 | - Added APIs for reading and writing teaching data 167 | - Added read_motion and write_motion_position 168 | 169 | Release 2.0.0 170 | 171 | - Method Names Renewal 172 | - Added Debug Mode 173 | - Added Data Acquisition on USB serial 174 | - Added Windows and Mac Support for USB serial 175 | -------------------------------------------------------------------------------- /pykeigan/utils.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import struct 4 | import sys 5 | 6 | def float2bytes(float_value): 7 | float_value=float(float_value) 8 | return struct.pack("!f", float_value) 9 | 10 | def bytes2float(byte_array): 11 | return struct.unpack('!f',byte_array)[0] 12 | 13 | def uint8_t2bytes(uint8_value): 14 | uint8_value=int(uint8_value) 15 | if uint8_value<0: 16 | raise TypeError("Argument should be positive or equal to zero") 17 | if uint8_value>256-1: 18 | raise TypeError("Argument should be less than 256") 19 | return struct.pack("B",uint8_value) 20 | 21 | def uint16_t2bytes(uint16_value): 22 | uint16_value=int(uint16_value) 23 | if uint16_value<0: 24 | raise TypeError("Argument should be positive or equal to zero") 25 | if uint16_value>256**2-1: 26 | raise TypeError("Argument should be less than 256**2") 27 | val1=int(uint16_value/256) 28 | val2=uint16_value-val1*256 29 | return struct.pack("BB",val1,val2) 30 | 31 | 32 | def uint16_t2bytes_little(uint16_value): 33 | uint16_value=int(uint16_value) 34 | if uint16_value<0: 35 | raise TypeError("Argument should be positive or equal to zero") 36 | if uint16_value>256**2-1: 37 | raise TypeError("Argument should be less than 256**2") 38 | val1=int(uint16_value/256) 39 | val2=uint16_value-val1*256 40 | return struct.pack("256**4-1: 47 | raise TypeError("Argument should be less than 256**4") 48 | val1=int(uint32_value/256**3) 49 | val2=int((uint32_value-val1*256**3)/256**2) 50 | val3=int((uint32_value-val1*256**3-val2*256**2)/256) 51 | val4=uint32_value-val1*256**3-val2*256**2-val3*256 52 | return struct.pack("BBBB",val1,val2,val3,val4) 53 | 54 | def bytes2uint32_t(ba): 55 | return struct.unpack(">I",ba)[0] 56 | 57 | def bytes2uint16_t(ba): 58 | return struct.unpack(">H", ba)[0] 59 | 60 | def bytes2uint8_t(ba): 61 | return struct.unpack("B",ba)[0] 62 | 63 | def bytes2int16_t(ba): 64 | return struct.unpack(">h",ba)[0] 65 | 66 | def bytes2int16_t_little(ba): 67 | return struct.unpack("> 8) 128 | 129 | return c 130 | 131 | def calc_crc16_bytes(buf): 132 | crc16 = calc_crc16(buf) 133 | #print(crc16) 134 | return uint16_t2bytes_little(crc16) 135 | 136 | -------------------------------------------------------------------------------- /pykeigan/blecontroller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Jan 10 09:13:24 2018 5 | 6 | @author: takata@innovotion.co.jp 7 | @author: harada@keigan.co.jp 8 | """ 9 | from pykeigan import controller as base 10 | import struct, time,threading, atexit 11 | from bluepy import btle 12 | from pykeigan.utils import * 13 | 14 | class BLEController(base.Controller): 15 | def __init__(self, addr,debug_mode=False): 16 | self.address = addr 17 | self.dev = btle.Peripheral(self.address, 'random') 18 | self.ble_lock = False 19 | for v in self.dev.getCharacteristics(): 20 | if v.uuid == 'f1400001-8936-4d35-a0ed-dfcd795baa8c': 21 | self.motor_tx_handle = v.getHandle() 22 | if v.uuid == 'f1400003-8936-4d35-a0ed-dfcd795baa8c': 23 | self.motor_led_handle = v.getHandle() 24 | if v.uuid == 'f1400004-8936-4d35-a0ed-dfcd795baa8c': 25 | self.motor_measurement_handle = v.getHandle() 26 | if v.uuid == 'f1400005-8936-4d35-a0ed-dfcd795baa8c': 27 | self.motor_imu_measurement_handle = v.getHandle() 28 | if v.uuid == 'f1400006-8936-4d35-a0ed-dfcd795baa8c': 29 | self.motor_rx_handle = v.getHandle() 30 | self.DebugMode=False 31 | if debug_mode: 32 | self.start_debug() 33 | self.set_interface(self.interface_type['BLE'] + self.interface_type['BTN']) 34 | 35 | def print_command_log(self,ba): 36 | print(self.command_names[bytes2uint8_t(ba[3:4])],self.error_codes[bytes2uint16_t(ba[7:9])]) 37 | 38 | def start_command_log_capturing(self): 39 | self.t = threading.Thread(target=self.__log_schedule_worker) 40 | self.t.setDaemon(True) 41 | self.t.start() 42 | atexit.register(self.__all_done) 43 | 44 | def __log_schedule_worker(self): 45 | old_ba = None 46 | while True: 47 | time.sleep(1) 48 | if not self.ble_lock: 49 | self.ble_lock=True 50 | ba = self.dev.readCharacteristic(self.motor_rx_handle) 51 | if bytes2uint8_t(ba[0:1]) == 0xBE and len(ba)==14: 52 | if ba!=old_ba: 53 | old_ba = ba 54 | self.print_command_log(ba) 55 | self.ble_lock=False 56 | if self.DebugMode == False: 57 | break 58 | 59 | def start_debug(self): 60 | """ 61 | Start to print command logs 62 | """ 63 | if self.DebugMode==False: 64 | self.start_command_log_capturing() 65 | self.DebugMode=True 66 | 67 | def finish_debug(self): 68 | """ 69 | Finish to print command logs 70 | """ 71 | self.DebugMode = False 72 | 73 | def __all_done(self): 74 | try: 75 | if self.t.isAlive(): 76 | self.t.join(0.01) 77 | except: 78 | return 79 | def _run_command(self, val, characteristics=None): 80 | crc_buf = calc_crc16_bytes(val) 81 | tx_buf = val + crc_buf 82 | while self.ble_lock: 83 | time.sleep(0.1) 84 | self.ble_lock = True 85 | if characteristics == 'motor_tx': 86 | self.dev.writeCharacteristic(self.motor_tx_handle, tx_buf) 87 | elif characteristics == 'motor_led': 88 | self.dev.writeCharacteristic(self.motor_led_handle, tx_buf) 89 | elif characteristics == 'motor_rx': 90 | self.dev.writeCharacteristic(self.motor_rx_handle, tx_buf) 91 | else: 92 | self.ble_lock = False 93 | raise ValueError('Invalid Characteristics') 94 | self.ble_lock = False 95 | def connect(self): 96 | """ 97 | Establish the BLE connection. 98 | """ 99 | self.dev.connect(self.address, 'random') 100 | 101 | def disconnect(self): 102 | """ 103 | Close the BLE connection. 104 | """ 105 | if self.DebugMode: 106 | self.DebugMode = False 107 | time.sleep(0.5) 108 | self.dev.disconnect() 109 | 110 | def read_motor_measurement(self): 111 | """ 112 | Get the position, velocity, and torque and store them to the properties 'position' in rad, 'velocity' in rad/sec, and 'torque' in N.m. 113 | """ 114 | while self.ble_lock: 115 | time.sleep(0.1) 116 | self.ble_lock = True 117 | ba = self.dev.readCharacteristic(self.motor_measurement_handle) 118 | error_count=0 119 | while len(ba)!=12: 120 | ba = self.dev.readCharacteristic(self.motor_measurement_handle) 121 | error_count+=1 122 | if error_count>10: 123 | raise ValueError("Unknown Error. Reading motor measurement values failed.") 124 | self.ble_lock = False 125 | position = bytes2float(ba[0:4]) 126 | velocity = bytes2float(ba[4:8]) 127 | torque = bytes2float(ba[8:12]) 128 | 129 | return {'position': position, 'velocity': velocity, 'torque': torque, 'received_unix_time': time.time()} 130 | 131 | def read_imu_measurement(self): 132 | """ 133 | Get the x,y,z axis acceleration, temperature, and anguler velocities around x,y,z axis 134 | and store them to 'accel_x', 'accel_y', 'accel_z' in g(9.80665 m/s^2), 'temp' in degree Celsius, 'gyro_x', 'gyro_y', and 'gyro_z' in rad/sec. Need to call enable_continual_imu_measurement() before calling this function. 135 | """ 136 | while self.ble_lock: 137 | time.sleep(0.1) 138 | self.ble_lock = True 139 | ba = self.dev.readCharacteristic(self.motor_imu_measurement_handle) 140 | error_count = 0 141 | while len(ba) != 14: 142 | ba = self.dev.readCharacteristic(self.motor_imu_measurement_handle) 143 | error_count+=1 144 | if error_count>10: 145 | raise ValueError("Reading imu values failed. Did you call enable_continual_imu_measurement() beforehand?") 146 | self.ble_lock = False 147 | accel_x = bytes2int16_t(ba[0:2]) * 2.0 / 32767 148 | accel_y = bytes2int16_t(ba[2:4]) * 2.0 / 32767 149 | accel_z = bytes2int16_t(ba[4:6]) * 2.0 / 32767 150 | temp = bytes2int16_t(ba[6:8]) / 333.87 + 21.00 151 | gyro_x = bytes2int16_t(ba[8:10]) * 0.00013316211 152 | gyro_y = bytes2int16_t(ba[10:12]) * 0.00013316211 153 | gyro_z = bytes2int16_t(ba[12:14]) * 0.00013316211 154 | return {'accel_x': accel_x, 'accel_y': accel_y, 'accel_z': accel_z, 'temp': temp, 'gyro_x': gyro_x, 155 | 'gyro_y': gyro_y, 'gyro_z': gyro_z, 'received_unix_time': time.time()} 156 | 157 | def __read_float_data(self, ba): 158 | return bytes2float(ba[4:8]) 159 | 160 | def __read_uint8_data(self, ba): 161 | return bytes2uint8_t(ba[4:5]) 162 | 163 | def __read_rgb_data(self, ba): 164 | return bytes2uint8_t(ba[4:5]), bytes2uint8_t(ba[5:6]),bytes2uint8_t(ba[6:7]) 165 | 166 | def __read_devicename_data(self, ba): 167 | return ba[4:17].decode('utf-8') 168 | 169 | def __read_deviceinfo_data(self, ba): 170 | return ba[4:35].decode('utf-8') 171 | 172 | def __read_status_data(self, ba): 173 | bits_list = [int(n) for n in bin(bytes2uint8_t(ba[4:5]))[2:].zfill(8)] 174 | return {"isCheckSumEnabled": bits_list[0], "iMUMeasurement": bits_list[4], "motorMeasurement": bits_list[5], 175 | "queue": bits_list[6], "motorEnabled": bits_list[7], 176 | "flash_memory_state": self.flash_memory_states[bytes2uint8_t(ba[5:6])], 177 | "motor_control_mode": self.motor_control_modes[bytes2uint8_t(ba[6:7])]} 178 | 179 | def _read_setting_value(self, comm): 180 | float_value_comms = [0x02, 0x03, 0x07, 0x08, 0x0E, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20, 0x21, 0x5B] 181 | valid_comms = [0x05, 0x3A, 0x46, 0x47, 0x9A] 182 | valid_comms.extend(float_value_comms) 183 | if not (comm in valid_comms): 184 | raise ValueError("Unknown Command") 185 | self.read_register(comm) 186 | while self.ble_lock: 187 | time.sleep(0.1) 188 | self.ble_lock = True 189 | ba = self.dev.readCharacteristic(self.motor_rx_handle) 190 | while len(ba) == 6 or bytes2uint8_t(ba[0:1]) == 0xBE: 191 | if bytes2uint8_t(ba[0:1]) == 0xBE: #in the case where the last command log remains 192 | self.read_register(comm) 193 | ba = self.dev.readCharacteristic(self.motor_rx_handle) 194 | self.ble_lock = False 195 | if comm in float_value_comms: 196 | return self.__read_float_data(ba) 197 | if comm == 0x05: 198 | return self.__read_uint8_data(ba) 199 | if comm == 0x3A: 200 | return self.__read_rgb_data(ba) 201 | if comm == 0x46: 202 | return self.__read_devicename_data(ba) 203 | if comm == 0x47: 204 | return self.__read_deviceinfo_data(ba) 205 | if comm == 0x9A: 206 | return self.__read_status_data(ba) 207 | return ba 208 | -------------------------------------------------------------------------------- /tools/log/log.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | from datetime import datetime 3 | import threading 4 | 5 | import logging 6 | import logging.handlers as handlers 7 | import shutil 8 | import os 9 | 10 | 11 | class MotorLog(object): 12 | def __init__(self, motor, fileName='', interval=1, overwriteCb=True): 13 | """ 14 | Initiate log / ログ生成 15 | :param motor: USBController 16 | motor to log 17 | ログ書き出しのモーター 18 | :param fileName: string, optional 19 | file name of log, default is port name of the motor (generated log file format: file_name.log.YYYY-mm-dd_HH-MM-SS) 20 | ログファイル名、デフォルトはモーターのポート名 (作成ログファイルフォーマット：file_name.log.YYYY-mm-dd_HH-MM-SS) 21 | :param interval: number, optional 22 | interval between getting data and writing log (sec), default is 1 23 | データ得る・ログ書き出し間隔（秒）、デフォルトは１ 24 | :param overwriteCb : boolean 25 | overwrite motor call back (on_motor_log_cb, on_motor_reconnection_cb, on_motor_connection_error_cb) to emit log, default is True 26 | モーターのコールバック (on_motor_log_cb, on_motor_reconnection_cb, on_motor_connection_error_cb) にログを書き出すように上書き、デフォルトはTrue 27 | """ 28 | print('\33[33m', 29 | "\n\nInitiate MotorLog (\'"+fileName + "\', "+motor.port + ", " + str( 30 | interval) + ", "+str(overwriteCb)+")\n", '\33[0m') 31 | 32 | if not os.path.exists('log'): 33 | os.makedirs('log') 34 | 35 | self.dev = motor 36 | self.interval = interval 37 | self.isConnected = True 38 | self.isLogging = False 39 | self.pause = False 40 | self.fileName = 'log/' 41 | if fileName=='': 42 | self.fileName += motor.port.split('/')[-1] 43 | else: 44 | self.fileName += fileName 45 | 46 | if overwriteCb: 47 | motor.on_motor_log_cb = self.overwriteMotorLogCb 48 | motor.on_motor_reconnection_cb = self.overwriteMotorReconnectionCb 49 | motor.on_motor_connection_error_cb = self.overwriteMotorConnectionErrCb 50 | 51 | 52 | self.logger = logging.getLogger(self.fileName) 53 | self.logger.setLevel(logging.DEBUG) 54 | logHandler = handlers.TimedRotatingFileHandler(self.fileName+'.log', when='H', interval=4, backupCount=120) 55 | #logHandler.suffix = "%Y%m%d-%H%M%S" 56 | logHandler.setLevel(logging.DEBUG) 57 | self.logger.addHandler(logHandler) 58 | 59 | header = self.fileName + "-" + self.dev.port + "(" + str(self.interval) + ")" 60 | if overwriteCb: 61 | header += "cb" 62 | self.logger.debug(header) 63 | 64 | def overwriteMotorLogCb(self, msg): 65 | """ 66 | Emit log when on_motor_log_cb (Internal use) 67 | on_motor_log_cbからログ書き出し（内側利用） 68 | :param msg: message from callback / コールバックからのメーセージ 69 | """ 70 | #print('log {} '.format(msg)) 71 | log = str(datetime.now()) + " | " + str(msg) 72 | self.logger.debug(log) 73 | 74 | def overwriteMotorReconnectionCb(self, msg): 75 | """ 76 | Emit log when on_motor_reconnection_cb (Internal use) 77 | on_motor_reconnection_cb（内側利用） 78 | :param msg: message from callback / コールバックからのメーセージ 79 | """ 80 | #print('log {} '.format(msg)) 81 | log = str(datetime.now()) + " | " + str(msg) 82 | self.logger.debug(log) 83 | 84 | def overwriteMotorConnectionErrCb(self, msg): 85 | """ 86 | Emit log when on_motor_connection_error_cb (Internal use) 87 | on_motor_connection_error_cb（内側利用） 88 | :param msg: message from callback / コールバックからのメーセージ 89 | """ 90 | #print('log {} '.format(msg)) 91 | log = str(datetime.now()) + " | " + str(msg) 92 | self.logger.debug(log) 93 | 94 | def startLogging(self): 95 | """ 96 | Start logging / ログ開始 97 | """ 98 | print('\33[33m', "\n\nStart logging (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 99 | log = str(datetime.now()) + " [START]" 100 | self.logger.debug(log) 101 | 102 | self.isLogging = True 103 | self.pause = False 104 | t = threading.Thread(target=self.readMotor) 105 | t.start() 106 | 107 | def pauseLogging(self): 108 | """ 109 | Pause logging / ログ一時停止 110 | Not available if logging is not started 111 | ログ開始していない状態は利用不可 112 | """ 113 | print('\33[33m', "Pause logging (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 114 | log = str(datetime.now()) + " [PAUSE]" 115 | self.logger.debug(log) 116 | if not self.isLogging: 117 | print('\33[33m', "[Error] logging is not started", '\33[0m') 118 | else: 119 | self.pause = True 120 | 121 | def resumeLogging(self): 122 | """ 123 | Resume logging / ログ再開 124 | Not available if logging is not started 125 | ログ開始していない状態は利用不可 126 | """ 127 | print('\33[33m', "Resume logging (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 128 | log = str(datetime.now()) + " [RESUME]" 129 | self.logger.debug(log) 130 | if not self.isLogging: 131 | print('\33[33m', "[Error] logging is not started", '\33[0m') 132 | #try: 133 | # print('\33[33m', "\tTry restart logging...", '\33[0m') 134 | # self.startLogging() 135 | #except: 136 | # print('\33[33m', "\t...cannot restart logging", '\33[0m') 137 | else: 138 | self.pause = False 139 | 140 | def stopLogging(self): 141 | """ 142 | Stop logging and backup latest log / ログ終了、最後のログバックアップ 143 | """ 144 | print('\33[33m', "\n\nStop logging (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 145 | log = str(datetime.now()) + " [STOP]" 146 | self.logger.debug(log) 147 | self.isLogging = False 148 | sleep(2) 149 | self.backupLastLog() 150 | 151 | def backupLastLog(self): 152 | """ 153 | Backup latest log / 最後のログバックアップ 154 | """ 155 | #latestLog = self.fileName + '.log.' + str(datetime.now().strftime('%Y%m%d-%H%M%S')) 156 | latestLog = self.fileName + '.log.' + str(datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) 157 | print('\33[33m', "back up latest log: " + latestLog, '\33[0m') 158 | shutil.copyfile(self.fileName + '.log', latestLog) 159 | 160 | def readMotor(self): 161 | """ 162 | Read motor data and write data to log / モーターデータ取得・ログに書き出し 163 | log format - Timestamp | {pos, vel, toq, unix, motor}, {enable, queue, ctrl} 164 | """ 165 | if not self.isLogging: 166 | print('\33[33m', "[Error] logging is not started (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 167 | 168 | while self.isLogging: 169 | if not self.pause: 170 | try: 171 | log = '' 172 | 173 | if not self.dev.is_connected(): 174 | log += str(datetime.now()) + " | " 175 | log += "Disconnected\n" 176 | self.isConnected = False 177 | elif not self.isConnected: 178 | log += str(datetime.now()) + " | " 179 | log += "Connected\n" 180 | self.isConnected = True 181 | 182 | try: 183 | log += str(datetime.now()) + " | " 184 | 185 | measurement = self.dev.read_motor_measurement() 186 | pos = str(round(measurement['position'], 3)) 187 | vel = str(round(measurement['velocity'], 3)) 188 | toq = str(round(measurement['torque'], 3)) 189 | unix = str(round(measurement['received_unix_time'], 3)) 190 | 191 | 192 | log +="{pos:"+pos+",vel:"+vel+",toq:"+toq+",unix:"+unix 193 | try: 194 | motor = str(measurement['motor_time']) 195 | log += ",motor:" + motor 196 | except: 197 | pass 198 | 199 | log +="}" 200 | 201 | status = self.dev.read_status() 202 | motor_enable = str(status['motorEnabled']) 203 | queue = str(status['queue']) 204 | ctrl = str(status['motor_control_mode']) 205 | 206 | log += "{enable:" + motor_enable + ",queue:" + queue + ",ctrl:" + ctrl + "}" 207 | 208 | #status {'isCheckSumEnabled': 1, 'iMUMeasurement': 0, 'motorMeasurement': 1, 'queue': 0, 'motorEnabled': 0, 'flash_memory_state': 'FLASH_STATE_READY', 'motor_control_mode': 'MOTOR_CONTROL_MODE_NONE'} 209 | #print(self.fileName+'\tstatus {} '.format(self.dev.read_status())) 210 | 211 | #measurement {'position': 12.35468578338623, 'velocity': 0.0037638440262526274, 'torque': 0.008983751758933067, 'received_unix_time': 1673331919.671698, 'motor_time': 2465588} 212 | #print(self.fileName+'\tmeasurement {} '.format(self.dev.read_motor_measurement())) 213 | 214 | 215 | 216 | except Exception as e: 217 | #print('\33[31m', e, '\33[0m') 218 | pass 219 | 220 | #print(log) 221 | self.logger.debug(log) 222 | 223 | sleep(self.interval) 224 | 225 | except: 226 | print('\33[33m', "\n\nLogging end by interrupt (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 227 | self.backupLastLog() 228 | return 229 | 230 | print('\33[33m', "\n\nLogging end (filename: "+self.fileName+", port:" + self.dev.port + ", interval:" + str(self.interval) + " s)\n", '\33[0m') 231 | 232 | def additionalLog(self, msg): 233 | """ 234 | Insert additional message to log / ログに追加メーセージ書き出し 235 | :param msg: string 236 | message to log 237 | """ 238 | log = str(datetime.now()) + " [add] | " + str(msg) 239 | self.logger.debug(log) 240 | -------------------------------------------------------------------------------- /pykeigan/usbcontroller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thu Jan 10 09:13:24 2018 5 | 6 | @author: takata@innovotion.co.jp 7 | @author: Hiroshi Harada (Keigan Inc.) 8 | @author: Takashi Tokuda (Keigan Inc.) 9 | """ 10 | from pykeigan import controller as base 11 | import serial, struct, threading, atexit, time 12 | from pykeigan.utils import * 13 | 14 | 15 | class USBController(base.Controller): 16 | def __init__(self, port='/dev/ttyUSB0',debug_mode=False, baud=115200, reconnect=True): 17 | self.DebugMode = debug_mode 18 | self.serial_buf = b'' # [] 19 | self.setting_values = {} 20 | self.__motor_measurement_value = None 21 | self.__imu_measurement_value = None 22 | self.__motor_log_value = None 23 | self.__motor_event_value = None 24 | self.__read_motion_value = [] 25 | self.worker=None 26 | self.port = port 27 | self.read_serial_polling_time = 0.004 28 | self.shouldReconnect = reconnect 29 | self.try_reconnect = False 30 | self.reconn_err_cnt = 0 31 | #try to connect to serial port 32 | self.serial = self.__init_serial(port, baud) 33 | 34 | self.on_motor_measurement_value_cb = False 35 | self.on_motor_imu_measurement_cb = False 36 | self.on_motor_connection_error_cb = False 37 | self.on_motor_reconnection_cb = False 38 | self.on_read_motion_read_comp_cb = False 39 | self.on_motor_log_cb = False 40 | self.on_motor_event_cb = False 41 | self.is_check_sum_enabled = False 42 | self.auto_serial_reading=False 43 | self.set_interface(self.interface_type['USB'] + self.interface_type['BTN']) 44 | atexit.register(self.my_cleanup) 45 | 46 | #precheck whether data from motor is avaliable or not 47 | #print('-- precheck reading value') 48 | try: 49 | self.start_auto_serial_reading() 50 | time.sleep(0.1) 51 | self._read_setting_value(0x46)#motor_name 52 | self._read_setting_value(0x47)#motor_info 53 | 54 | except Exception as e: 55 | print('Precheck error: '+str(e)) 56 | print('\tattemping to reconnect...') 57 | time.sleep(1) 58 | print('...reconnecting...') 59 | #self.disconnect() 60 | #self.__init__() 61 | 62 | def __init_serial(self, port, baud): 63 | while True: 64 | try: 65 | return serial.Serial(port, baud, 8, 'N', 1, None, False, True, write_timeout=0.1) 66 | except Exception as e: 67 | print('Error occured while trying to connect to serial port. Please recheck you USB connection.') 68 | print('attemping to reinit... ') 69 | time.sleep(3) 70 | 71 | def is_connected(self): 72 | return self.serial.isOpen() 73 | 74 | def connect(self): 75 | """ 76 | Open the USB port. 77 | Should be after disconnection. 78 | """ 79 | self.serial.open() 80 | self.start_auto_serial_reading() 81 | time.sleep(0.1) 82 | 83 | def recover(self): 84 | self.start_auto_serial_reading() 85 | 86 | def disconnect(self, reconnect=False): 87 | """ 88 | Close the USB port. 89 | """ 90 | self.shouldReconnect = reconnect 91 | self.my_cleanup() 92 | time.sleep(0.5) 93 | self.serial.close() 94 | 95 | def reconnect(self): 96 | if not self.shouldReconnect or self.try_reconnect: return 97 | self.disconnect(self.shouldReconnect) 98 | self.reconn_err_cnt=0 99 | self.try_reconnect = True 100 | 101 | while True: 102 | self.reconn_err_cnt += 1 103 | #print('Try reconnecting to : ', self.port," (",self.reconn_err_cnt,")") 104 | print('Try reconnecting to : '+str(self.port)+" ("+ str(self.reconn_err_cnt)+ ")") 105 | try: 106 | self.serial.open() 107 | self.set_interface(self.interface_type['USB'] + self.interface_type['BTN']) 108 | except Exception as e: 109 | # print(e) 110 | pass 111 | time.sleep(1) 112 | try: 113 | #self.connect() 114 | rd = self.serial.read(self.serial.inWaiting()) 115 | self.try_reconnect=False 116 | self.start_auto_serial_reading() 117 | if (callable(self.on_motor_reconnection_cb)): 118 | self.on_motor_reconnection_cb(self.reconn_err_cnt) 119 | break 120 | except serial.SerialException as e: 121 | print('Serial reconnection Failed.', self.reconn_err_cnt,e) 122 | continue 123 | except TypeError as e: 124 | continue 125 | except IOError as e: 126 | print('Serial reconnection Failed.', self.reconn_err_cnt,e) 127 | continue 128 | except Exception as e: 129 | break 130 | 131 | self.try_reconnect=False 132 | 133 | def start_debug(self): 134 | """ 135 | Start to print command logs. 136 | """ 137 | self.DebugMode = True 138 | 139 | def finish_debug(self): 140 | """ 141 | Finish to print command logs. 142 | """ 143 | self.DebugMode = False 144 | 145 | def start_auto_serial_reading(self): 146 | if self.auto_serial_reading: 147 | return 148 | self.auto_serial_reading = True 149 | if self.worker is None: 150 | self.worker = threading.Thread(target=self.__serial_schedule_worker) 151 | self.worker.setDaemon(True) 152 | self.worker.start() 153 | atexit.register(self.__all_done) 154 | 155 | def finish_auto_serial_reading(self): 156 | self.auto_serial_reading = False 157 | 158 | def __all_done(self): 159 | try: 160 | if self.worker.isAlive(): 161 | self.worker.join(0.01) 162 | self.serial.close() 163 | except: 164 | return 165 | 166 | def _run_command(self, val, characteristics=None): 167 | try: 168 | crc_buf = calc_crc16_bytes(val) 169 | tx_buf = val+crc_buf 170 | self.serial.write(tx_buf) 171 | except serial.SerialException as e: 172 | # There is no new data from serial port 173 | self.reconnect() 174 | if (callable(self.on_motor_connection_error_cb)): 175 | self.on_motor_connection_error_cb(e) 176 | return e 177 | except TypeError as e: 178 | # Disconnect of USB->UART occured 179 | self.reconnect() 180 | if (callable(self.on_motor_connection_error_cb)): 181 | self.on_motor_connection_error_cb(e) 182 | return e 183 | except IOError as e: 184 | self.reconnect() 185 | if (callable(self.on_motor_connection_error_cb)): 186 | self.on_motor_connection_error_cb(e) 187 | return e 188 | 189 | def __serial_schedule_worker(self): 190 | while True: 191 | time.sleep(self.read_serial_polling_time) # less than minimum motor measurement interval 192 | if self.auto_serial_reading: 193 | e_res = self.__read_serial_data() 194 | else: 195 | print("stop auto_serial_reading") 196 | 197 | def __read_serial_data(self): 198 | try: 199 | #print(self.serial.in_waiting, self.serial.inWaiting()) 200 | rd = self.serial.read(self.serial.inWaiting()) 201 | except serial.SerialException as e: 202 | print('serial.SerialException in __read_serial_data: ', e) 203 | # There is no new data from serial port 204 | if (callable(self.on_motor_connection_error_cb)): 205 | self.on_motor_connection_error_cb(e) 206 | self.reconnect() 207 | return e 208 | except TypeError as e: 209 | # Disconnect of USB->UART occured 210 | print('TypeError in __read_serial_data: ', e) 211 | 212 | if (callable(self.on_motor_connection_error_cb)): 213 | self.on_motor_connection_error_cb(e) 214 | self.reconnect() 215 | return e 216 | except IOError as e: 217 | print('IOError in __read_serial_data: ', e) 218 | 219 | if (callable(self.on_motor_connection_error_cb)): 220 | self.on_motor_connection_error_cb(e) 221 | self.reconnect() 222 | return e 223 | 224 | self.serial_buf += rd 225 | 226 | """ 227 | ------------------------------ 228 | preamble detection logic 229 | ------------------------------ 230 | """ 231 | bf_len = len(self.serial_buf) 232 | is_pre = False 233 | if (bf_len < 8): 234 | return 235 | 236 | slice_idx = bf_len 237 | success = False 238 | i = 0 239 | 240 | while i < bf_len-3: 241 | # preamble detection 242 | if not is_pre and self.serial_buf[i:i+4] == b'\x00\x00\xaa\xaa': 243 | is_pre = True 244 | slice_idx = i 245 | for ie in range(i + 4, bf_len-3): 246 | # postamble detection 247 | if self.serial_buf[ie + 2:ie+4] == b'\x0d\x0a': 248 | payload = self.serial_buf[i + 4: ie] 249 | # CRC Verification 250 | buf_to_validate = self.serial_buf[i + 4: ie + 2] 251 | crc = calc_crc16(buf_to_validate) 252 | 253 | if self.is_check_sum_enabled: 254 | if calc_crc16(buf_to_validate) == 0: 255 | success = self.__serialdataParseSafe(payload) 256 | else: 257 | success = self.__serialdataParseSafe(payload) 258 | slice_idx = ie + 4 259 | i = ie + 3 260 | is_pre = False 261 | break 262 | i += 1 263 | self.serial_buf = self.serial_buf[slice_idx:] 264 | 265 | def __serialdataParseSafe(self, byte_array): 266 | try: 267 | return self.__serialdataParse(byte_array) 268 | except Exception as e: 269 | return False 270 | 271 | def __serialdataParse(self, byte_array): 272 | v_len = len(byte_array) 273 | if (v_len < 3 or bytes2uint8_t(byte_array[0:1]) != v_len): 274 | return False 275 | datatype = bytes2uint8_t(byte_array[1:2]) 276 | payload = byte_array[2:] 277 | if datatype == 0xB4: # モーター回転情報受信 278 | position = bytes2float(payload[0:4]) 279 | velocity = bytes2float(payload[4:8]) 280 | torque = bytes2float(payload[8:12]) 281 | self.__motor_measurement_value = {'position': position, 'velocity': velocity, 'torque': torque, 282 | 'received_unix_time': time.time()} 283 | if (callable(self.on_motor_measurement_value_cb)): 284 | self.on_motor_measurement_value_cb(self.__motor_measurement_value) 285 | return True 286 | elif datatype == 0xBA: # モーター時刻+回転情報受信 モーターFW ver 2.62 287 | motor_time = bytes2uint32_t(payload[0:4]) 288 | position = bytes2float(payload[4:8]) 289 | velocity = bytes2float(payload[8:12]) 290 | torque = bytes2float(payload[12:16]) 291 | self.__motor_measurement_value = {'position': position, 'velocity': velocity, 'torque': torque, 292 | 'received_unix_time': time.time(), 'motor_time': motor_time} 293 | if (callable(self.on_motor_measurement_value_cb)): 294 | self.on_motor_measurement_value_cb(self.__motor_measurement_value) 295 | return True 296 | elif datatype == 0xB5: # IMU情報受信 297 | accel_x = bytes2int16_t(payload[0:2]) * 2.0 / 32767 298 | accel_y = bytes2int16_t(payload[2:4]) * 2.0 / 32767 299 | accel_z = bytes2int16_t(payload[4:6]) * 2.0 / 32767 300 | temp = bytes2int16_t(payload[6:8]) / 333.87 + 21.00 301 | gyro_x = bytes2int16_t(payload[8:10]) * 0.00013316211 302 | gyro_y = bytes2int16_t(payload[10:12]) * 0.00013316211 303 | gyro_z = bytes2int16_t(payload[12:14]) * 0.00013316211 304 | self.__imu_measurement_value = {'accel_x': accel_x, 'accel_y': accel_y, 'accel_z': accel_z, 'temp': temp, 305 | 'gyro_x': gyro_x, 'gyro_y': gyro_y, 'gyro_z': gyro_z, 306 | 'received_unix_time': time.time()} 307 | if (callable(self.on_motor_imu_measurement_cb)): 308 | self.on_motor_imu_measurement_cb(self.__imu_measurement_value) 309 | return True 310 | elif datatype == 0x40: #Register infomations 311 | float_value_comms = [0x02, 0x03, 0x07, 0x08, 0x0E, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20, 312 | 0x21, 0x5B] 313 | comm = bytes2uint8_t(payload[2:3]) 314 | if comm in float_value_comms: 315 | self.setting_values[comm] = bytes2float(payload[3:7]), time.time() 316 | return True 317 | elif comm == 0x05: 318 | self.setting_values[comm] = bytes2uint8_t(payload[3:4]), time.time() 319 | return True 320 | elif comm == 0x3A: 321 | self.setting_values[comm] = (bytes2uint8_t(payload[3:4]), bytes2uint8_t(payload[4:5]), bytes2uint8_t(payload[5:6])), time.time() 322 | return True 323 | elif comm == 0x46: 324 | self.setting_values[comm] = payload[3:16].decode('utf-8'), time.time() 325 | return True 326 | elif comm == 0x47: 327 | self.setting_values[comm] = payload[3:40].decode('utf-8'), time.time() 328 | return True 329 | elif comm == 0x9A: 330 | bits_list = [int(n) for n in bin(bytes2uint8_t(payload[3:4]))[2:].zfill(8)] 331 | self.setting_values[comm] = {"isCheckSumEnabled": bits_list[0], "iMUMeasurement": bits_list[4], 332 | "motorMeasurement": bits_list[5], "queue": bits_list[6], 333 | "motorEnabled": bits_list[7], 334 | "flash_memory_state": self.flash_memory_states[ 335 | bytes2uint8_t(payload[4:5])], 336 | "motor_control_mode": self.motor_control_modes[ 337 | bytes2uint8_t(payload[5:6])]}, time.time() 338 | return True 339 | else: 340 | return False 341 | elif datatype == 0xBE: # command log (Error or Success information) 342 | #print(payload) 343 | cmd = bytes2uint8_t(payload[2:3]) 344 | err_code = bytes2uint32_t(payload[3:7]) 345 | try: 346 | cmd_name = self.command_names[cmd] 347 | except KeyError: 348 | print('[Error info] No such command exists. cmd = ', hex(cmd)) 349 | return True 350 | try: 351 | err_desc = self.error_codes[err_code] 352 | # print(err_code) 353 | except KeyError: 354 | print('[Error info] No such error_code exists. error_code = ', err_code) 355 | return True 356 | self.__motor_log_value={'command_names':cmd,'error_codes':err_code} 357 | if (callable(self.on_motor_log_cb)): 358 | self.on_motor_log_cb(self.__motor_log_value) 359 | if self.DebugMode: 360 | print(self.__motor_log_value['command_names'],self.__motor_log_value['error_codes']) 361 | return True 362 | elif datatype == 0xB7: #Position coordinates of "readMotion" (Motor firmware ver>=2.0) 363 | motion_index=bytes2uint16_t(payload[0:2]) 364 | total_pos_len = bytes2uint32_t(payload[2:6]) 365 | payload_fast_pos_idx=bytes2uint32_t(payload[6:10]) 366 | payload_pos_len=bytes2uint32_t(payload[10:14]) 367 | ar=[] 368 | for i in range(payload_pos_len): 369 | try: 370 | ar.append(bytes2float(payload[14+(i*4):18+(i*4)])) 371 | except: 372 | break 373 | try: 374 | del self.__read_motion_value[payload_fast_pos_idx:] 375 | except: 376 | pass 377 | self.__read_motion_value.extend(ar) 378 | 379 | if(len(self.__read_motion_value) >= total_pos_len): #Completed receiving all data 380 | if (callable(self.on_read_motion_read_comp_cb)): 381 | self.on_read_motion_read_comp_cb(motion_index, self._read_motion_value()) 382 | return True 383 | elif datatype == 0xCE: # Event notification from KeiganMotor. (MotorFarmVar >2.28) 384 | self.__motor_event_value={'event_type':self.event_types[bytes2uint8_t(payload[0:1])],'number':bytes2uint8_t(payload[1:2]),'state':bytes2uint8_t(payload[2:3])} 385 | if (callable(self.on_motor_event_cb)): 386 | self.on_motor_event_cb(self.__motor_event_value) 387 | 388 | return True 389 | else: # Unknown data 390 | return False 391 | 392 | def _read_setting_value(self, comm, validation_threshold=1.0):#Register infomation 393 | float_value_comms = [0x02, 0x03, 0x07, 0x08, 0x0E, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20, 0x21, 394 | 0x5B] 395 | valid_comms = [0x05, 0x3A, 0x46, 0x47, 0x9A] 396 | valid_comms.extend(float_value_comms) 397 | if not (comm in valid_comms): 398 | raise ValueError("Unknown Command") 399 | self.read_register(comm) 400 | time.sleep(0.15) 401 | if not self.auto_serial_reading: 402 | raise ValueError("Disabled reading serial data. Try calling start_auto_serial_reading()") 403 | if comm in self.setting_values.keys(): 404 | val, received_unix_time = self.setting_values[comm] 405 | print("rev time", time.time() - received_unix_time) 406 | return val 407 | else: 408 | raise ValueError("No data received") 409 | 410 | def __read_measurement_value(self, comm, validation_threshold=1.0): 411 | measurement_value=None 412 | if not (comm in [0xB4, 0xB5]): 413 | raise ValueError("Unknown Command") 414 | if not self.auto_serial_reading: 415 | raise ValueError("Disabled reading serial data. Try calling start_serial_reading().") 416 | if comm == 0xB4: 417 | measurement_value = self.__motor_measurement_value 418 | elif comm == 0xB5: 419 | measurement_value = self.__imu_measurement_value 420 | if measurement_value is None: 421 | if comm == 0xB4: 422 | raise ValueError("No data received. Try calling enable_continual_motor_measurement().") 423 | if comm == 0xB5: 424 | raise ValueError("No data received. Try calling enable_continual_imu_measurement().") 425 | elif time.time() - measurement_value['received_unix_time'] < validation_threshold: 426 | return measurement_value 427 | else: 428 | if comm == 0xB4: 429 | raise ValueError("No data within ", validation_threshold, " sec.") 430 | if comm == 0xB5: 431 | raise ValueError("No data within ", validation_threshold, " sec.") 432 | 433 | def _read_motion_value(self): 434 | return self.__read_motion_value 435 | 436 | def read_motor_measurement(self): 437 | return self.__read_measurement_value(0xB4) 438 | 439 | def read_imu_measurement(self): 440 | return self.__read_measurement_value(0xB5) 441 | 442 | def my_cleanup(self): 443 | self.finish_auto_serial_reading() 444 | -------------------------------------------------------------------------------- /pykeigan/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Thr Jan 10 09:13:24 2018 5 | 6 | @author: takata@innovotion.co.jp 7 | @author: harada@keigan.co.jp 8 | """ 9 | import serial,struct,threading,atexit,time 10 | from pykeigan.utils import * 11 | 12 | class Controller: 13 | def __init__(self): 14 | pass 15 | 16 | def _run_command(self,val,characteristics): 17 | pass 18 | @property 19 | def flash_memory_states(self): 20 | return {0:"FLASH_STATE_READY",1:"FLASH_STATE_TEACHING_PREPARE",2:"FLASH_STATE_TEACHING_DOING",3:"FLASH_STATE_PLAYBACK_PREPARE",4:"FLASH_STATE_PLAYBACK_DOING",5:"FLASH_STATE_PLAYBACK_PAUSING",6:"FLASH_STATE_TASKSET_RECORDING",7:"FLASH_STATE_TASKSET_DOING",8:"FLASH_STATE_TASKSET_PAUSING",20:"FLASH_STATE_IMU"} 21 | 22 | @property 23 | def motor_control_modes(self): 24 | return {0:"MOTOR_CONTROL_MODE_NONE",1:"MOTOR_CONTROL_MODE_VELOCITY",2:"MOTOR_CONTROL_MODE_POSITION",3:"MOTOR_CONTROL_MODE_TORQUE",0xFF:"MOTOR_CONTROL_MODE_OTHERS"} 25 | 26 | @property 27 | def baud_rates(self): 28 | return {0:"115200",1:"230400",2:"250000",3:"460800",4:"921600",5:"1000000"} 29 | 30 | @property 31 | def error_codes(self): 32 | return { 33 | 0x00:"KM_SUCCESS", 34 | 0x03:"KM_ERROR_NOT_FOUND", 35 | 0x05:"KM_ERROR_INVALID_COMMAND", 36 | 0x06:"KM_ERROR_INVALID_PARAM", 37 | 0x07:"KM_ERROR_STORAGE_FULL", 38 | 0x08:"KM_ERROR_INVALID_FLASH_STATE", 39 | 0x09:"KM_ERROR_INVALID_LENGTH", 40 | 0x0A:"KM_ERROR_INVALID_CHECKSUM", 41 | 0x0F:"KM_ERROR_FORBIDDEN", 42 | 0x10:"KM_ERROR_INVALID_ADDR", 43 | 0x14:"KM_ERROR_MOTOR_DISABLED", 44 | 0x46:"KM_ERROR_NRF_DEVICE", 45 | 0x50:"KM_ERROR_WDT_EVENT", 46 | 0x51:"KM_ERROR_OVER_HEAT", 47 | 0x64:"KM_SUCCESS_ARRIVAL" 48 | } 49 | 50 | @property 51 | def command_names(self): 52 | return { 53 | 0x00:"unknown", 54 | 0x02:"set_max_speed", 55 | 0x03:"set_min_speed", 56 | 0x05:"set_curve_type", 57 | 0x07:"set_acc", 58 | 0x08:"set_dec", 59 | 0x0e:"set_max_torque", 60 | 0x16:"set_teaching_interval", 61 | 0x17:"set_playback_interval", 62 | 0x18:"set_qcurrent_p", 63 | 0x19:"set_qcurrent_i", 64 | 0x1A:"set_qcurrent_d", 65 | 0x1B:"set_speed_p", 66 | 0x1C:"set_speed_i", 67 | 0x1D:"set_speed_d", 68 | 0x1E:"set_position_p", 69 | 0x1F:"set_position_i", 70 | 0x20:"set_position_d", 71 | 0x21:"set_pos_control_threshold", 72 | 0x22:"reset_all_pid", 73 | 0x23:"set_pid_table_value", 74 | 0x24:"select_pid_table", 75 | 0x25:"read_pid_table", 76 | 0x27:"set_low_pass_filter", 77 | 0x2B:"set_notify_pos_arrival_settings", 78 | 0x2C:"set_motor_measurement_interval", 79 | 0x2D:"set_motor_measurement_settings", 80 | 0x2E:"set_interface", 81 | 0x30:"set_response", 82 | 0x31:"set_safe_run_settings", 83 | 0x32:"set_safety_settings", 84 | 0x33:"set_limit_current", 85 | 0x37:"set_linkage_key", 86 | 0x3A:"set_own_color", 87 | 0x3C:"set_imu_measurement_interval", 88 | 0x3D:"set_imu_measurement_settings", 89 | 0x40:"read_register", 90 | 0x41:"save_all_registers", 91 | 0x46:"read_device_name", 92 | 0x47:"read_device_info", 93 | 0x4E:"reset_register", 94 | 0x4F:"reset_all_registers", 95 | 0x50:"disable_action", 96 | 0x51:"enable_action", 97 | 0x58:"set_speed", 98 | 0x5A:"preset_position", 99 | 0x5B:"get_position_offset", 100 | 0x60:"run_forward", 101 | 0x61:"run_reverse", 102 | 0x62:"run_at_velocity", 103 | 0x65:"move_to_pos", 104 | 0x66:"move_to_pos", 105 | 0x67:"move_by_dist", 106 | 0x68:"move_by_dist", 107 | 0x6A:"enable_action_task", 108 | 0x6B:"disable_action_task", 109 | 0x6C:"free_motor", 110 | 0x6D:"stop_motor", 111 | 0x72:"hold_torque", 112 | 0x75:"move_to_pos_wait", 113 | 0x76:"move_to_pos_wait", 114 | 0x77:"move_by_dist_wait", 115 | 0x78:"move_by_dist_wait", 116 | 0x81:"start_doing_taskset", 117 | 0x82:"stop_doing_taskset", 118 | 0x85:"start_playback_motion", 119 | 0x86:"prepare_playback_motion", 120 | 0x87:"start_playback_motion_from_prep", 121 | 0x88:"stop_playback_motion", 122 | 0x90:"pause_queue", 123 | 0x91:"resume_queue", 124 | 0x92:"wait_queue", 125 | 0x94:"erase_task", 126 | 0x95:"clear_queue", 127 | 0x96:"wait_queue_until_input", 128 | 0x9A:"read_status", 129 | 0xA0:"start_recording_taskset", 130 | 0xA2:"stop_recording_taskset", 131 | 0xA3:"erase_taskset", 132 | 0xA4:"erase_all_tasksets", 133 | 0xA5:"set_taskset_name", 134 | 0xA6:"read_taskset_info", 135 | 0xA7:"read_taskset_usage", 136 | 0xA8:"set_trigger_taskset_settings", 137 | 0xA9:"start_teaching_motion", 138 | 0xAA:"prepare_teaching_motion", 139 | 0xAB:"start_teaching_motion_from_prep", 140 | 0xAC:"stop_teaching_motion", 141 | 0xAD:"erase_motion", 142 | 0xAE:"erase_all_motions", 143 | 0xAF:"set_motion_name", 144 | 0xB0:"read_motion_info", 145 | 0xB1:"read_motion_usage", 146 | 0xB2:"set_trigger_motion_settings", 147 | 0xB7:"read_motion", 148 | 0xB8:"write_motion_position", 149 | 0xBC:"set_autostart_setting", 150 | 0xBD:"set_button_setting", 151 | 0xBE:"read_error", 152 | 0xC0:"set_i2c_address", 153 | 0xC3:"set_baud_rate", 154 | 0xE0:"set_led", 155 | 0xE6:"enable_motor_measurement", 156 | 0xE7:"disable_motor_measurement", 157 | 0xEA:"enable_imu_measurement", 158 | 0xEB:"disable_imu_measurement", 159 | 0xF0:"reboot", 160 | 0xF3:"enable_check_sum" 161 | } 162 | 163 | @property 164 | def event_types(self): 165 | return {1:"button", 10:"gpio"} 166 | 167 | # Settings 168 | def set_max_speed(self,max_speed,identifier=b'\x00\x00'): 169 | """ 170 | Set the maximum speed of rotation to the 'max_speed' in rad/sec. 171 | """ 172 | command=b'\x02' 173 | values=float2bytes(max_speed) 174 | self._run_command(command+identifier+values,'motor_rx') 175 | 176 | def set_min_speed(self,min_speed,identifier=b'\x00\x00'): 177 | """ 178 | Set the minimum speed of rotation to the 'min_speed' in rad/sec. 179 | """ 180 | command=b'\x03' 181 | values=float2bytes(min_speed) 182 | self._run_command(command+identifier+values,'motor_rx') 183 | 184 | def set_curve_type(self,curve_type,identifier=b'\x00\x00'): 185 | """ 186 | Set the acceleration or deceleration curve to the 'curve_type'. 187 | typedef enum curveType = 188 | { 189 | CURVE_TYPE_NONE = 0, // Turn off Motion control 190 | CURVE_TYPE_TRAPEZOID = 1, // Turn on Motion control with trapezoidal curve 191 | CURVE_TYPE_DIRECT_POS = 10 // Turn off Motion control (Direct position control) 192 | } 193 | """ 194 | command=b'\x05' 195 | values=uint8_t2bytes(curve_type) 196 | self._run_command(command+identifier+values,'motor_rx') 197 | 198 | def set_acc(self,_acc,identifier=b'\x00\x00'): 199 | """ 200 | Set the acceleration of rotation to the positive 'acc' in rad/sec^2. 201 | """ 202 | command=b'\x07' 203 | values=float2bytes(_acc) 204 | self._run_command(command+identifier+values,'motor_rx') 205 | 206 | def set_dec(self,_dec,identifier=b'\x00\x00'): 207 | """ 208 | Set the deceleration of rotation to the positive 'dec' in rad/sec^2. 209 | """ 210 | command=b'\x08' 211 | values=float2bytes(_dec) 212 | self._run_command(command+identifier+values,'motor_rx') 213 | 214 | def set_max_torque(self,max_torque,identifier=b'\x00\x00'): 215 | """ 216 | Set the maximum torque to the positive 'max_torque' in N.m. 217 | """ 218 | command=b'\x0E' 219 | if max_torque < 0: 220 | raise ValueError("Value out of range") 221 | values=float2bytes(max_torque) 222 | self._run_command(command+identifier+values,'motor_rx') 223 | 224 | def set_teaching_interval(self,interval_ms,identifier=b'\x00\x00'): 225 | """ 226 | Set the interval time of teaching to the positive integer "interval_ms" in ms. 227 | """ 228 | command=b'\x16' 229 | values=uint32_t2bytes(interval_ms) 230 | self._run_command(command+identifier+values,'motor_rx') 231 | 232 | def set_playback_interval(self,interval_ms,identifier=b'\x00\x00'): 233 | """ 234 | Set the interval time of playback to the positive integer "interval_ms" in ms. 235 | """ 236 | command=b'\x17' 237 | values=uint32_t2bytes(interval_ms) 238 | self._run_command(command+identifier+values,'motor_rx') 239 | 240 | def set_qcurrent_p(self,q_current_p,identifier=b'\x00\x00'): 241 | """ 242 | Set the q-axis current PID controller's Proportional gain to the postiive 'q_current_p'. 243 | """ 244 | command=b'\x18' 245 | values=float2bytes(q_current_p) 246 | self._run_command(command+identifier+values,'motor_rx') 247 | 248 | def set_qcurrent_i(self,q_current_i,identifier=b'\x00\x00'): 249 | """ 250 | Set the q-axis current PID controller's Integral gain to the positive 'q_current_i'. 251 | """ 252 | command=b'\x19' 253 | values=float2bytes(q_current_i) 254 | self._run_command(command+identifier+values,'motor_rx') 255 | 256 | def set_qcurrent_d(self,q_current_d,identifier=b'\x00\x00'): 257 | """ 258 | Set the q-axis current PID controller's Differential gain to the postiive 'q_current_d'. 259 | """ 260 | command=b'\x1A' 261 | values=float2bytes(q_current_d) 262 | self._run_command(command+identifier+values,'motor_rx') 263 | 264 | def set_speed_p(self,speed_p,identifier=b'\x00\x00'): 265 | """ 266 | Set the speed PID controller's Proportional gain to the positive 'speed_p'. 267 | """ 268 | command=b'\x1B' 269 | values=float2bytes(speed_p) 270 | self._run_command(command+identifier+values,'motor_rx') 271 | 272 | def set_speed_i(self,speed_i,identifier=b'\x00\x00'): 273 | """ 274 | Set the speed PID controller's Integral gain to the positive 'speed_i'. 275 | """ 276 | command=b'\x1C' 277 | values=float2bytes(speed_i) 278 | self._run_command(command+identifier+values,'motor_rx') 279 | 280 | def set_speed_d(self,speed_d,identifier=b'\x00\x00'): 281 | """ 282 | Set the speed PID controller's Deferential gain to the positive 'speed_d'. 283 | """ 284 | command=b'\x1D' 285 | values=float2bytes(speed_d) 286 | self._run_command(command+identifier+values,'motor_rx') 287 | 288 | def set_position_p(self,position_p,identifier=b'\x00\x00'): 289 | """ 290 | Set the position PID controller's Proportional gain to the positive 'position_p'. 291 | """ 292 | command=b'\x1E' 293 | values=float2bytes(position_p) 294 | self._run_command(command+identifier+values,'motor_rx') 295 | 296 | def set_position_i(self,position_i,identifier=b'\x00\x00'): 297 | """ 298 | Set the position PID controller's Integral gain to the positive 'position_i'. 299 | """ 300 | command=b'\x1F' 301 | values=float2bytes(position_i) 302 | self._run_command(command+identifier+values,'motor_rx') 303 | 304 | def set_position_d(self,position_d,identifier=b'\x00\x00'): 305 | """ 306 | Set the position PID controller's Differential gain to the positive 'position_d'. 307 | """ 308 | command=b'\x20' 309 | values=float2bytes(position_d) 310 | self._run_command(command+identifier+values,'motor_rx') 311 | 312 | def set_pos_control_threshold(self,poscontrol_d,identifier=b'\x00\x00'): 313 | """ 314 | Set the threshold of the deviation between the target and current position. While the deviation is more than the threshold, integral and differential gain is set to be zero. 315 | """ 316 | command=b'\x21' 317 | values=float2bytes(poscontrol_d) 318 | self._run_command(command+identifier+values,'motor_rx') 319 | 320 | def reset_all_pid(self,identifier=b'\x00\x00'): 321 | """ 322 | Reset all the PID parameters to the firmware default settings. 323 | """ 324 | command=b'\x22' 325 | self._run_command(command+identifier,'motor_rx') 326 | 327 | def set_motor_measurement_interval(self,interval,identifier=b'\x00\x00'): 328 | """ 329 | Set the notification interval of motor measurement values. It is valid only for USB(UART) and BLE. 330 | In case of BLE, the allowed minimum value is 100 [ms]. 331 | ----------- 332 | 0: 2 [ms] 333 | 1: 5 [ms] 334 | 2: 10 [ms] 335 | 3: 20 [ms] 336 | 4: 50 [ms] 337 | 5: 100 [ms] 338 | 6: 200 [ms] 339 | 7: 500 [ms] 340 | 8: 1000 [ms] 341 | 9: 2000 [ms] 342 | 10: 5000 [ms] 343 | 11: 10000 [ms] 344 | ----------- 345 | """ 346 | command=b'\x2C' 347 | values = uint8_t2bytes(interval) 348 | self._run_command(command+identifier+values,'motor_rx') 349 | 350 | def set_motor_measurement_settings(self,flag,identifier=b'\x00\x00'): 351 | """ 352 | Set the notification setting of motor measurement values. 353 | ----------- 354 | bit7: - 355 | bit6: - 356 | bit5: - 357 | bit4: - 358 | bit3: - 359 | bit2: - 360 | bit1: Notification of motor measurement (1:ON, 0:OFF) 361 | bit0: To start Notification of motor measurement when booting(1:ON, 0:OFF) 362 | ----------- 363 | """ 364 | command=b'\x2D' 365 | values=uint8_t2bytes(flag) 366 | self._run_command(command+identifier+values,'motor_rx') 367 | 368 | def set_limit_current(self,limit,identifier=b'\x00\x00'): 369 | 370 | """ 371 | Set current limit threshold to cut off. It is based on Iq current. 372 | Please use this function if you are using 373 | """ 374 | 375 | command=b'\x33' 376 | values=float2bytes(limit) 377 | 378 | self._run_command(command+identifier+values,'motor_rx') 379 | 380 | @property 381 | def interface_type(self): 382 | return { 383 | "BLE": 0b1, 384 | "USB": 0b1000, 385 | "I2C": 0b10000, 386 | "BTN": 0b10000000, 387 | } 388 | 389 | def set_interface(self,flag,identifier=b'\x00\x00'): 390 | """ 391 | You can enable or disable the data interfaces(physical 3 buttons, I2C, USB and BLE). 392 | The motor chooses the output interface of motor measurement values and IMU values as 393 | (High priority)BLE > USB > I2C(Low priority) by default. 394 | If you want to force it to send measurement values through USB, 395 | you need to set bit0(BLE) to OFF(0) and bit3(USB) to ON(1). 396 | For example, if you call set_interface(0b10001000), Physical 3 buttons: enabled, I2C: disabled, USB: enabled and BLE: disabled. 397 | To save this setting to the flash memory, ensure you call saveAllRegisters().@n 398 | ----------- 399 | - bit7: Physical 3 buttons 400 | - bit6: UART2 401 | - bit5: Digital In 402 | - bit4: I2C(Wired) 403 | - bit3: UART1(USB, Wired) 404 | - bit2: - 405 | - bit1: Linkage 406 | - bit0: BLE(Wireless) 407 | ----------- 408 | """ 409 | command=b'\x2E' 410 | values=uint8_t2bytes(flag) 411 | self._run_command(command+identifier+values,'motor_rx') 412 | 413 | def set_safe_run_settings(self,isEnabled,timeout,stopAction,identifier=b'\x00\x00'): 414 | """set_safe_run_settings 415 | 416 | Set safe run mode to stop motor automatically when it cannot receive next command within a certain period (timeout). 417 | 418 | Args: 419 | isEnabled (bool): true if setting safe run mode enabled 420 | timeout (int): stop action will occur after this period 421 | stopAction (int): 422 | - 0: free_motor 423 | - 1: disable_motor_action 424 | - 2: stop_motor 425 | - 3: Fix to the current position (move_to) 426 | """ 427 | command=b'\x31' 428 | values=uint8_t2bytes(isEnabled)+uint32_t2bytes(timeout)+uint8_t2bytes(stopAction) 429 | self._run_command(command+identifier+values,'motor_rx') 430 | 431 | def set_own_color(self,red,green,blue,identifier=b'\x00\x00'): 432 | """ 433 | Set the own LED color(red:0-255, green:0-255, blue:0-255). 434 | """ 435 | command=b'\x3A' 436 | values=uint8_t2bytes(red)+uint8_t2bytes(green)+uint8_t2bytes(blue) 437 | self._run_command(command+identifier+values,'motor_rx') 438 | 439 | def set_imu_measurement_interval(self,interval,identifier=b'\x00\x00'): 440 | """ 441 | Set the notification interval of IMU measurement values. It is valid only for USB(UART) and BLE. 442 | In case of BLE, the allowed minimum value is 100 [ms]. 443 | ----------- 444 | 0: 10 [ms] 445 | 1: 20 [ms] 446 | 2: 50 [ms] 447 | 3: 100 [ms] 448 | 4: 200 [ms] 449 | 5: 500 [ms] 450 | 6: 1000 [ms] 451 | 7: 2000 [ms] 452 | 9: 5000 [ms] 453 | 10: 10000 [ms] 454 | ----------- 455 | """ 456 | command=b'\x3C' 457 | values = uint8_t2bytes(interval) 458 | self._run_command(command+identifier+values,'motor_rx') 459 | 460 | def set_imu_measurement_settings(self,flag,identifier=b'\x00\x00'): 461 | """ 462 | Set the notification setting of IMU. 463 | ----------- 464 | bit7: - 465 | bit6: - 466 | bit5: - 467 | bit4: - 468 | bit3: - 469 | bit2: - 470 | bit1: Notification of IMU (1:ON, 0:OFF) 471 | bit0: To start Notification of IMU when booting(1:ON, 0:OFF) 472 | ----------- 473 | """ 474 | command=b'\x3D' 475 | values=uint8_t2bytes(flag) 476 | self._run_command(command+identifier+values,'motor_rx') 477 | 478 | def set_autostart_setting(self, mode, identifier=b'\x00\x00'): 479 | """ 480 | Set the button setting (Motor Firmware Ver. requires >= 2.42) 481 | ----------- 482 | mode = 0: Without auto start 483 | mode = 1: Auto start doing taskset (config by set_taskset_trigger_settings) 484 | mode = 2: Auto start playback motion (config by set_motion_trigger_settings) 485 | ----------- 486 | """ 487 | command=b'\xBC' 488 | values=uint8_t2bytes(mode) 489 | self._run_command(command+identifier+values,'motor_rx') 490 | 491 | 492 | def set_button_setting(self, mode, identifier=b'\x00\x00'): 493 | """ 494 | Set the button setting (MotorFirmVer. >= 2.28) 495 | ----------- 496 | mode = 0: Manual button mode with Motion 497 | mode = 1: Manual button mode with Taskset 498 | mode = 30: Child button mode (Receive button action event from KeiganMotor) 499 | ----------- 500 | """ 501 | command=b'\xBD' 502 | values=uint8_t2bytes(mode) 503 | self._run_command(command+identifier+values,'motor_rx') 504 | 505 | def set_baud_rate(self, baud, identifier=b'\x00\x00'): 506 | """ 507 | Set baud rate for serial communication setting 508 | (baud rate unit is kbps) 509 | ----------- 510 | 0: 115200 511 | 1: 230400 512 | 2: 250000 513 | 3: 460800 514 | 4: 921600 515 | 5: 1000000 (1M) 516 | ----------- 517 | """ 518 | command=b'\xC3' 519 | values=uint8_t2bytes(baud) 520 | self._run_command(command+identifier+values,'motor_rx') 521 | 522 | 523 | def set_notify_pos_arrival_settings(self, isEnabled, tolerance, settleTime, identifier=b'\x00\x00'): 524 | """ 525 | Set notification settings when arriving at the target position 526 | """ 527 | command=b'\x2B' 528 | values=uint8_t2bytes(isEnabled)+float2bytes(tolerance)+uint32_t2bytes(settleTime) 529 | self._run_command(command+identifier+values,'motor_rx') 530 | 531 | 532 | def read_register(self,register,identifier=b'\x00\x00'): 533 | ''' 534 | Read a specified setting (register). 535 | ''' 536 | command=b'\x40' 537 | values=uint8_t2bytes(register) 538 | self._run_command(command+identifier+values,'motor_rx') 539 | 540 | def save_all_registers(self,identifier=b'\x00\x00'): 541 | """ 542 | Save all settings (registers) in flash memory. 543 | """ 544 | command=b'\x41' 545 | self._run_command(command+identifier,'motor_rx') 546 | time.sleep(3) # wait for next command to store registers in flash 547 | 548 | def reset_register(self,register,identifier=b'\x00\x00'): 549 | """ 550 | Reset a specified register's value to the firmware default setting. 551 | """ 552 | command=b'\x4E' 553 | values=uint8_t2bytes(register) 554 | self._run_command(command+identifier+values,'motor_rx') 555 | 556 | def reset_all_registers(self,identifier=b'\x00\x00'): 557 | """ 558 | Reset all registers' values to the firmware default setting. 559 | """ 560 | command=b'\x4F' 561 | self._run_command(command+identifier,'motor_rx') 562 | 563 | # Motor Action 564 | def disable_action(self,identifier=b'\x00\x00'): 565 | """ 566 | Disable motor action. 567 | """ 568 | command=b'\x50' 569 | self._run_command(command+identifier,'motor_tx') 570 | 571 | def enable_action(self,identifier=b'\x00\x00'): 572 | """ 573 | Enable motor action. 574 | """ 575 | command=b'\x51' 576 | self._run_command(command+identifier,'motor_tx') 577 | 578 | def set_speed(self,speed,identifier=b'\x00\x00'): 579 | """ 580 | Set the speed of rotation to the positive 'speed' in rad/sec. 581 | """ 582 | command=b'\x58' 583 | values=float2bytes(speed) 584 | self._run_command(command+identifier+values,'motor_tx') 585 | 586 | def preset_position(self,position,identifier=b'\x00\x00'): 587 | """ 588 | Preset the current absolute position as the specified 'position' in rad. (Set it to zero when setting origin) 589 | """ 590 | command=b'\x5A' 591 | values=float2bytes(position) 592 | self._run_command(command+identifier+values,'motor_tx') 593 | 594 | def get_position_offset(self,position,identifier=b'\x00\x00'): 595 | """ 596 | Get the offset value of the absolute position. 597 | """ 598 | command=b'\x5B' 599 | values=float2bytes(position) 600 | self._run_command(command+identifier+values,'motor_tx') 601 | 602 | def run_forward(self,identifier=b'\x00\x00'): 603 | """ 604 | Rotate the motor forward (counter clock-wise) at the speed set by 0x58: speed. 605 | """ 606 | command=b'\x60' 607 | self._run_command(command+identifier,'motor_tx') 608 | 609 | def run_reverse(self,identifier=b'\x00\x00'): 610 | """ 611 | Rotate the motor reverse (clock-wise) at the speed set by 0x58: speed. 612 | """ 613 | command=b'\x61' 614 | self._run_command(command+identifier,'motor_tx') 615 | 616 | def run_at_velocity(self,velocity,identifier=b'\x00\x00'): 617 | """ 618 | Rotate the motor at the 'velocity'. The velocity can be positive or negative. 619 | """ 620 | command=b'\x62' 621 | values=float2bytes(velocity) 622 | self._run_command(command+identifier+values,'motor_tx') 623 | 624 | def move_to_pos(self,position,speed=None,identifier=b'\x00\x00'): 625 | """ 626 | Move the motor to the specified absolute 'position' at the 'speed'. 627 | If the speed is None, move at the speed set by 0x58: set_speed. 628 | """ 629 | if speed is not None: 630 | command=b'\x65' 631 | values=float2bytes(position)+float2bytes(speed) 632 | else: 633 | command=b'\x66' 634 | values=float2bytes(position) 635 | self._run_command(command+identifier+values,'motor_tx') 636 | 637 | def move_by_dist(self,distance,speed=None,identifier=b'\x00\x00'): 638 | """ 639 | Move the motor by the specified relative 'distance' from the current position at the 'speed'. 640 | If the speed is None, move at the speed set by 0x58: set_speed. 641 | """ 642 | if speed is not None: 643 | command=b'\x67' 644 | values=float2bytes(distance)+float2bytes(speed) 645 | else: 646 | command=b'\x68' 647 | values=float2bytes(distance) 648 | self._run_command(command+identifier+values,'motor_tx') 649 | 650 | def move_to_pos_wait(self,position,speed=None,identifier=b'\x00\x00'): 651 | """ 652 | Move the motor to the specified absolute 'position' at the 'speed'. 653 | It pauses the queue to handle commands in motor side at the same time. 654 | "Queue" will resume automatically if it arrives at the target position. 655 | See "set_notify_pos_arrival_settings" to change parameters such as the tolerance. 656 | If the speed is None, move at the speed set by 0x58: set_speed. 657 | """ 658 | if speed is not None: 659 | command=b'\x75' 660 | values=float2bytes(position)+float2bytes(speed) 661 | else: 662 | command=b'\x76' 663 | values=float2bytes(position) 664 | self._run_command(command+identifier+values,'motor_tx') 665 | 666 | def move_by_dist_wait(self,distance,speed=None,identifier=b'\x00\x00'): 667 | """ 668 | Move the motor by the specified relative 'distance' from the current position at the 'speed'. 669 | It pauses the queue to handle commands in motor side at the same time. 670 | The queue will be resumed automatically if it arrives at the target position. 671 | See "set_notify_pos_arrival_settings" to change parameters such as the tolerance. 672 | If the speed is None, move at the speed set by 0x58: set_speed. 673 | """ 674 | if speed is not None: 675 | command=b'\x77' 676 | values=float2bytes(distance)+float2bytes(speed) 677 | else: 678 | command=b'\x78' 679 | values=float2bytes(distance) 680 | self._run_command(command+identifier+values,'motor_tx') 681 | 682 | def disable_action_task(self,identifier=b'\x00\x00'): 683 | """ 684 | Disable motor action. (Firmware ver. requires >= 2.42) 685 | This function can be stored in taskset, while disable_action() cannot be stored. 686 | """ 687 | command=b'\x6A' 688 | self._run_command(command+identifier,'motor_tx') 689 | 690 | def enable_action_task(self,identifier=b'\x00\x00'): 691 | """ 692 | Enable motor action. (Firmware ver. requires >= 2.42) 693 | This function can be stored in taskset, while enable_action() cannot be stored. 694 | """ 695 | command=b'\x6B' 696 | self._run_command(command+identifier,'motor_tx') 697 | 698 | def free_motor(self,identifier=b'\x00\x00'): 699 | """ 700 | Stop the motor's excitation 701 | """ 702 | command=b'\x6C' 703 | self._run_command(command+identifier,'motor_tx') 704 | 705 | def stop_motor(self,identifier=b'\x00\x00'): 706 | """ 707 | Decelerate the speed to zero and stop. 708 | """ 709 | command=b'\x6D' 710 | self._run_command(command+identifier,'motor_tx') 711 | 712 | def hold_torque(self,torque,identifier=b'\x00\x00'): 713 | """ 714 | Keep and output the specified torque. 715 | """ 716 | command=b'\x72' 717 | values=float2bytes(torque) 718 | self._run_command(command+identifier+values,'motor_tx') 719 | 720 | def start_doing_taskset(self,index,repeating,identifier=b'\x00\x00'): 721 | """ 722 | Do taskset at the specified 'index' 'repeating' times. 723 | """ 724 | command=b'\x81' 725 | values=uint16_t2bytes(index)+uint32_t2bytes(repeating) 726 | self._run_command(command+identifier+values,'motor_tx') 727 | 728 | def stop_doing_taskset(self,identifier=b'\x00\x00'): 729 | """ 730 | Stop doing the current taskset. 731 | """ 732 | command=b'\x82' 733 | self._run_command(command+identifier,'motor_tx') 734 | 735 | def start_playback_motion(self,index,repeating,option,identifier=b'\x00\x00'): 736 | """ 737 | Start to playback motion at the specified 'index' 'repeating' times. 738 | """ 739 | command=b'\x85' 740 | values=uint16_t2bytes(index)+uint32_t2bytes(repeating)+uint8_t2bytes(option) 741 | self._run_command(command+identifier+values,'motor_tx') 742 | 743 | def prepare_playback_motion(self,index,repeating,option,identifier=b'\x00\x00'): 744 | """ 745 | Prepare to playback motion at the specified 'index' 'repeating' times. 746 | """ 747 | command=b'\x86' 748 | values=uint16_t2bytes(index)+uint32_t2bytes(repeating)+uint8_t2bytes(option) 749 | self._run_command(command+identifier+values,'motor_tx') 750 | 751 | def start_playback_motion_from_prep(self,identifier=b'\x00\x00'): 752 | """ 753 | Start to playback motion in the condition of the last preparePlaybackMotion. 754 | """ 755 | command=b'\x87' 756 | self._run_command(command+identifier,'motor_tx') 757 | 758 | def stop_playback_motion(self,identifier=b'\x00\x00'): 759 | """ 760 | Stop to playback motion. 761 | """ 762 | command=b'\x88' 763 | self._run_command(command+identifier,'motor_tx') 764 | 765 | # Queue 766 | def pause_queue(self,identifier=b'\x00\x00'): 767 | """ 768 | Pause the queue until 0x91: resume is executed. 769 | """ 770 | command=b'\x90' 771 | self._run_command(command+identifier,'motor_tx') 772 | 773 | def resume_queue(self,identifier=b'\x00\x00'): 774 | """ 775 | Resume the queue. 776 | """ 777 | command=b'\x91' 778 | self._run_command(command+identifier,'motor_tx') 779 | 780 | def wait_queue(self,waittime,identifier=b'\x00\x00'): 781 | """ 782 | Wait the queue or pause the queue for the specified 'time' in msec and resume it automatically. 783 | """ 784 | command=b'\x92' 785 | values=uint32_t2bytes(waittime) 786 | self._run_command(command+identifier+values,'motor_tx') 787 | 788 | def clear_queue(self,identifier=b'\x00\x00'): 789 | """ 790 | Clear the queue. Erase all tasks in the queue. This command works when 0x90: pause or 0x92: wait are executed. 791 | """ 792 | command=b'\x95' 793 | self._run_command(command+identifier,'motor_tx') 794 | 795 | 796 | def wait_queue_until_input(self, pin, event=2, timeout=0, sub_pin1=0xFF, sub_state1=0, sub_pin2=0xFF, sub_state2=0, sub_pin3=0xFF, sub_state3=0, identifier=b'\x00\x00'): 797 | """ 798 | Wait the queue or pause the queue for the specified GPIO digital pin event. 799 | It is resumd automatically if timeout[ms] passed. 800 | event parameter is as follows. 801 | ----------- 802 | 1: RISING (Low to High) 803 | 2: FALLING (High to Low) 804 | ----------- 805 | """ 806 | command=b'\x96' 807 | values=uint8_t2bytes(pin)+uint8_t2bytes(event)+uint32_t2bytes(timeout)+uint8_t2bytes(sub_pin1)+uint8_t2bytes(sub_state1)+uint8_t2bytes(sub_pin2)+uint8_t2bytes(sub_state2)+uint8_t2bytes(sub_pin3)+uint8_t2bytes(sub_state3) 808 | self._run_command(command+identifier+values,'motor_tx') 809 | 810 | 811 | # Taskset 812 | def start_recording_taskset(self,index,identifier=b'\x00\x00'): 813 | """ 814 | Start recording taskset at the specified 'index' in the flash memory. 815 | In the case of KM-1, index value is from 0 to 49 (50 in total). 816 | """ 817 | command=b'\xA0' 818 | values=uint16_t2bytes(index) 819 | self._run_command(command+identifier+values,'motor_tx') 820 | 821 | def stop_recording_taskset(self,identifier=b'\x00\x00'): 822 | """ 823 | Stop recording taskset. 824 | This command works while 0xA0: startRecordingTaskset is executed. 825 | """ 826 | command=b'\xA2' 827 | self._run_command(command+identifier,'motor_tx') 828 | 829 | def erase_taskset(self,index,identifier=b'\x00\x00'): 830 | """ 831 | Erase taskset at the specified index in the flash memory. 832 | In the case of KM-1, index value is from 0 to 49 (50 in total). 833 | """ 834 | command=b'\xA3' 835 | values=uint16_t2bytes(index) 836 | self._run_command(command+identifier+values,'motor_tx') 837 | 838 | def erase_all_tasksets(self,identifier=b'\x00\x00'): 839 | """ 840 | Erase all tasksets in the flash memory. 841 | """ 842 | command=b'\xA4' 843 | self._run_command(command+identifier,'motor_tx') 844 | 845 | def set_trigger_taskset_settings(self,index,repeating,identifier=b'\x00\x00'): 846 | """ 847 | Set taskset settings by trigger such as button 848 | """ 849 | command=b'\xA8' 850 | values=uint16_t2bytes(index)+uint32_t2bytes(repeating) 851 | self._run_command(command+identifier+values,'motor_tx') 852 | 853 | 854 | # Teaching 855 | def start_teaching_motion(self,index,time_ms,identifier=b'\x00\x00'): 856 | """ 857 | Start teaching motion without preparing by specifying the 'index' in the flash memory and recording 'time' in milliseconds. 858 | In the case of KM-1, index value is from 0 to 9 (10 in total). Recording time cannot exceed 65408 [msec]. 859 | """ 860 | command=b'\xA9' 861 | values=uint16_t2bytes(index)+uint32_t2bytes(time_ms) 862 | self._run_command(command+identifier+values,'motor_tx') 863 | 864 | def prepare_teaching_motion(self,index,time_ms,identifier=b'\x00\x00'): 865 | """ 866 | Prepare teaching motion by specifying the 'index' in the flash memory and recording 'time' in milliseconds. 867 | In the case of KM-1, index value is from 0 to 9 (10 in total). Recording time cannot exceed 65408 [msec]. 868 | """ 869 | command=b'\xAA' 870 | values=uint16_t2bytes(index)+uint32_t2bytes(time_ms) 871 | self._run_command(command+identifier+values,'motor_tx') 872 | 873 | def start_teaching_motion_from_prep(self,identifier=b'\x00\x00'): 874 | """ 875 | Start teaching motion in the condition of the last prepareTeachingMotion. 876 | This command works when the teaching index is specified by 0xAA: prepareTeachingMotion. 877 | """ 878 | command=b'\xAB' 879 | self._run_command(command+identifier,'motor_tx') 880 | 881 | def stop_teaching_motion(self,identifier=b'\x00\x00'): 882 | """ 883 | Stop teaching motion. 884 | """ 885 | command=b'\xAC' 886 | self._run_command(command+identifier,'motor_tx') 887 | 888 | def erase_motion(self,index,identifier=b'\x00\x00'): 889 | """ 890 | Erase motion at the specified index in the flash memory. 891 | In the case of KM-1, index value is from 0 to 9 (10 in total). 892 | """ 893 | command=b'\xAD' 894 | values=uint16_t2bytes(index) 895 | self._run_command(command+identifier+values,'motor_tx') 896 | 897 | def erase_all_motions(self,identifier=b'\x00\x00'): 898 | """ 899 | Erase all motions in the flash memory. 900 | """ 901 | command=b'\xAE' 902 | self._run_command(command+identifier,'motor_tx') 903 | 904 | def set_trigger_motion_settings(self,index,repeating,option,autoStart,identifier=b'\x00\x00'): 905 | """ 906 | Set playback motion settings by trigger such as button 907 | """ 908 | command=b'\xB2' 909 | values=uint16_t2bytes(index)+uint32_t2bytes(repeating)+uint8_t2bytes(option)+uint8_t2bytes(autoStart) 910 | self._run_command(command+identifier+values,'motor_tx') 911 | 912 | def read_motion(self,index,read_comp_cb,identifier=b'\x00\x00'): 913 | """ 914 | Externally output the motion stored by direct teaching. (Motor farm ver>=2.0) 915 | It is valid only for a wired connection(USB). 916 | callback signature 917 | read_comp_cb(motion_index,motion_value[]) 918 | """ 919 | command=b'\xB7' 920 | values = uint16_t2bytes(index) 921 | self._run_command(command + identifier + values, 'motor_tx') 922 | self.on_read_motion_read_comp_cb=read_comp_cb 923 | return True 924 | 925 | def write_motion_position(self,position,identifier=b'\x00\x00'): 926 | """ 927 | Record one coordinate of movement from the outside. (Motor farm ver>=2.0) 928 | It is valid only for a wired connection(USB). 929 | ※ This command can not be accepted unless it is in prepareTeaching state.What to do after running "prepareTeachingMotion" 930 | ※ This command records coordinates one by one. If one record is made, it will be waiting to record the next coordinate. 931 | ※ After recording coordinates with this command, recording can not be completed normally unless stopTeaching is executed. 932 | """ 933 | command=b'\xB8' 934 | values = float2bytes(position) 935 | self._run_command(command + identifier + values, 'motor_tx') 936 | 937 | # LED 938 | def set_led(self,ledState,red,green,blue,identifier=b'\x00\x00'): 939 | """ 940 | Set the LED state (off, solid, flash and dim) and color intensity (red, green and blue). 941 | typedef enum ledState = 942 | { 943 | LED_STATE_OFF = 0, // LED off 944 | LED_STATE_ON_SOLID = 1, // LED solid 945 | LED_STATE_ON_FLASH = 2, // LED flash 946 | LED_STATE_ON_DIM = 3 // LED dim 947 | } 948 | """ 949 | command=b'\xE0' 950 | values=uint8_t2bytes(ledState)+uint8_t2bytes(red)+uint8_t2bytes(green)+uint8_t2bytes(blue) 951 | self._run_command(command+identifier+values,"motor_tx") 952 | 953 | def enable_continual_motor_measurement(self,identifier=b'\x00\x00'): 954 | """ 955 | Enable the motor measurement and start continual notification of the measurement values. 956 | """ 957 | command=b'\xE6' 958 | self._run_command(command+identifier,'motor_tx') 959 | 960 | def disable_continual_motor_measurement(self,identifier=b'\x00\x00'): 961 | """ 962 | Disable the motor measurement and stop notification of the measurement values. 963 | """ 964 | command=b'\xE7' 965 | self._run_command(command+identifier,'motor_tx') 966 | 967 | # IMU 968 | def enable_continual_imu_measurement(self,identifier=b'\x00\x00'): 969 | """ 970 | Enable the IMU and start notification of the measurement values. 971 | """ 972 | command=b'\xEA' 973 | self._run_command(command+identifier,'motor_tx') 974 | 975 | def disable_continual_imu_measurement(self,identifier=b'\x00\x00'): 976 | """ 977 | Disable the IMU and stop notification of the measurement values. 978 | """ 979 | command=b'\xEB' 980 | self._run_command(command+identifier,'motor_tx') 981 | 982 | # System 983 | def reboot(self,identifier=b'\x00\x00'): 984 | """ 985 | Reboot the system. 986 | """ 987 | command=b'\xF0' 988 | self._run_command(command+identifier,'motor_tx') 989 | 990 | def enable_check_sum(self,isEnabled,identifier=b'\x00\x00'): 991 | command=b'\xF3' 992 | values=uint8_t2bytes(isEnabled) 993 | self._run_command(command+identifier+values,'motor_tx') 994 | 995 | def enter_device_firmware_update(self,identifier=b'\x00\x00'): 996 | """ 997 | Enter the device firmware update mode or bootloader mode. It goes with reboot. 998 | """ 999 | command=b'\xFD' 1000 | self._run_command(command+identifier,'motor_tx') 1001 | 1002 | def read_max_speed(self): 1003 | """ 1004 | Read the current maximum speed of rotation 'max_speed' in rad/sec. 1005 | """ 1006 | return self._read_setting_value(0x02) 1007 | 1008 | def read_min_speed(self): 1009 | """ 1010 | Read the current minimum speed of rotation 'min_speed' in rad/sec. 1011 | """ 1012 | return self._read_setting_value(0x03) 1013 | 1014 | def read_curve_type(self): 1015 | """ 1016 | Read the current acceleration or deceleration curve 'curve_type'. 1017 | typedef enum curveType = 1018 | { 1019 | CURVE_TYPE_NONE = 0, // Turn off Motion control 1020 | CURVE_TYPE_TRAPEZOID = 1, // Turn on Motion control with trapezoidal curve 1021 | } 1022 | """ 1023 | return self._read_setting_value(0x05) 1024 | 1025 | def read_acc(self): 1026 | """ 1027 | Read the current acceleration of rotation 'acc' in rad/sec^2. 1028 | """ 1029 | return self._read_setting_value(0x07) 1030 | 1031 | def read_dec(self): 1032 | """ 1033 | Read the current deceleration of rotation 'acc' in rad/sec^2. 1034 | """ 1035 | return self._read_setting_value(0x08) 1036 | 1037 | def read_max_torque(self): 1038 | """ 1039 | Read the current maximum torque 'max_torque' in N.m. 1040 | """ 1041 | return self._read_setting_value(0x0E) 1042 | 1043 | def read_qcurrent_p(self): 1044 | """ 1045 | Read the q-axis current PID controller's Proportional gain 'q_current_p'. 1046 | """ 1047 | return self._read_setting_value(0x18) 1048 | 1049 | def read_qcurrent_i(self): 1050 | """ 1051 | Read the q-axis current PID controller's Integral gain 'q_current_i'. 1052 | """ 1053 | return self._read_setting_value(0x19) 1054 | 1055 | def read_qcurrent_d(self): 1056 | """ 1057 | Read the q-axis current PID controller's Differential gain 'q_current_d'. 1058 | """ 1059 | return self._read_setting_value(0x1A) 1060 | 1061 | def read_speed_p(self): 1062 | """ 1063 | Read the speed PID controller's Proportional gain 'speed_p'. 1064 | """ 1065 | return self._read_setting_value(0x1B) 1066 | 1067 | def read_speed_i(self): 1068 | """ 1069 | Read the speed PID controller's Integral gain 'speed_i'. 1070 | """ 1071 | return self._read_setting_value(0x1C) 1072 | 1073 | def read_speed_d(self): 1074 | """ 1075 | Read the speed PID controller's Differential gain 'speed_d'. 1076 | """ 1077 | return self._read_setting_value(0x1D) 1078 | 1079 | def read_position_p(self): 1080 | """ 1081 | Read the position PID controller's Proportional gain 'position_p'. 1082 | """ 1083 | return self._read_setting_value(0x1E) 1084 | 1085 | def read_position_i(self): 1086 | """ 1087 | Read the position PID controller's Integral gain 'position_i'. 1088 | """ 1089 | return self._read_setting_value(0x1F) 1090 | 1091 | def read_position_d(self): 1092 | """ 1093 | Read the position PID controller's Differential gain 'position_d'. 1094 | """ 1095 | return self._read_setting_value(0x20) 1096 | 1097 | def read_pos_control_threshold(self): 1098 | """ 1099 | Read the threshold of the deviation between the target and current position. While the deviation is more than the threshold, integral and differential gain is set to be zero. 1100 | """ 1101 | return self._read_setting_value(0x21) 1102 | 1103 | def read_own_color(self): 1104 | """ 1105 | ToDo 1106 | Read the own LED color. Return (red,green,blue). 1107 | """ 1108 | return self._read_setting_value(0x3A) 1109 | 1110 | def read_device_name(self): 1111 | """ 1112 | Read the device name of the motor. 1113 | """ 1114 | return self._read_setting_value(0x46) 1115 | 1116 | def read_device_info(self): 1117 | """ 1118 | Get the device information of the motor. 1119 | """ 1120 | return self._read_setting_value(0x47) 1121 | 1122 | def read_status(self): 1123 | """ 1124 | Read the motor status: 'isCheckSumEnabled', 'iMUMeasurement', 'motorMeasurement', 'queue', 'motorEnabled', 'flash_memory_state', 'motor_control_mode'. 1125 | """ 1126 | return self._read_setting_value(0x9A) 1127 | 1128 | def read_baud_rate(self): 1129 | """ 1130 | Get the baud rate of UART(USB). 1131 | """ 1132 | return self._read_setting_value(0xC3) 1133 | 1134 | def read_motor_measurement_interval(self): 1135 | """ 1136 | Get the motor measurement interval. 1137 | """ 1138 | return self._read_setting_value(0x2C) 1139 | 1140 | def read_imu_measurement_interval(self): 1141 | """ 1142 | Get the motor measurement interval. 1143 | """ 1144 | return self._read_setting_value(0x3C) 1145 | 1146 | 1147 | def _read_setting_value(self, comm):#dummy 1148 | pass 1149 | def _read_motion_value(self):#dummy 1150 | pass 1151 | 1152 | def read_gpio_status(self): 1153 | """ 1154 | Read the GPIO status of the motor. 1155 | """ 1156 | return self._read_setting_value(0xCC) 1157 | --------------------------------------------------------------------------------