def __init__(self, port_left, port_right, safe_time = 1, safe_option = 1, wheel_d = 100, tread = 400, button_event_cb = None):
self.left = usbcontroller.USBController(port_left,False)
self.right = usbcontroller.USBController(port_right,False)
self.safe_time = safe_time
self.safe_option = safe_option
self.curve(0)
self.safe_setting(True)
self.round_num = tread / wheel_d # 360度回転するのに必要な回転数
self.button_setting(button_event_cb)
def motor_init(self):
parser = argparse.ArgumentParser(description='モーターに接続し、各種情報の取得')
parser.add_argument('port', metavar='PORT', default='/dev/ttyUSB1', nargs='?',
help='モーターのデバイスファイル指定 (default:/dev/ttyUSB1)')
args = parser.parse_args()
self.dev = usbcontroller.USBController(args.port, False, baud=1000000) # モーターのアドレス 参照 usb-simple-connection.py
print("tsunagatta")
self.dev.disable_action()
self.dev.set_max_speed(utils.deg2rad(1000))
time.sleep(2)
self.dev.enable_action()
self.dev.preset_position(0.0)
self.dev.set_curve_type(1)
self.dev.set_acc(100)
self.dev.set_led(2, 0, 255, 255)
self.dev.set_speed(utils.deg2rad(180))
for m in self.modules.values():
m.dev = self.dev
self.sensor.dev = self.dev
self.dev.set_max_torque(0.03)
#self.dev.move_to_pos(utils.deg2rad(-40), (utils.deg2rad(90) / 3))
#time.sleep(3)
#self.dev.move_to_pos(0.01)
#time.sleep(3)
"""self.dev.set_acc(200)
def connect(self, port):
if sys.platform.startswith('win'): #detects if platform is windows and uses the input port
# !attention assumes pyserial 3.x
self.dev = usbcontroller.USBController(port, baud=115200) #run select port if windows
print("Connected.")
elif sys.platform.startswith('linux') or sys.platform.startswith('cygwin'): #if platform is linux, uses default linux port for keiganmotor
# this excludes your current terminal "/dev/tty"
self.dev = usbcontroller.USBController("/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_DM00KIWP-if00-port0",False)
print("Connected.")
self.dev.enable_action()
self.dev.set_notify_pos_arrival_settings(False, 0, 0) #avoids position read errors
self.dev.set_led(1, 255, 255, 255)
self.dev.enable_continual_motor_measurement()
self.dev.enable_continual_imu_measurement()
self.dev.set_motor_measurement_interval(2)
self.dev.run_at_velocity(3) #runs for troubleshooting -- can see that the motor connected
sleep(1)
self.dev.stop_motor()
from pykeigan import usbcontroller
from pykeigan import utils
# How to use (this sample was made for Windows)
# (this sample was made for Windows)
#
# (1) Define port 'COM?' that connected to KeiganMotor (You can see by DeviceManager)
# (2) Set index to record and do taskset (selectable from 0 to 49)
# (3) Set repeat to repeat when doing taskset (0~, 0 will repeat infinitely)
# (4) Excute this file
# (5) Press 'r' to record my_taskset in KeiganMotor
# (6) Press 'd' to do taskset
# Select port connecting to KeiganMotor
port = 'COM7'
dev = usbcontroller.USBController(port)
index = 0 # index to record and do taskset
repeat = 1 # repeating number to do taskset
# Edit this function to record your taskset
# (Recorded in KeiganMotor)
def my_taskset():
dev.set_speed(utils.rpm2rad_per_sec(5))
dev.set_led(1, 0, 200, 0)
dev.run_forward()
dev.wait_queue(5000) # wait for 5000 ms
dev.stop_motor()
print('Conncted to', portdev)
return portdev
def baud_rate_setting():
print('Select baud rate to set')
print('--------')
print('0: 115200')
print('1: 230400')
print('2: 250000')
print('3: 460800')
print('4: 921600')
print('5: 1000000 (1M)')
print('--------')
num = int(input())
while num < 0 or num > 5:
print('Invalid value!')
num = int(input())
return num
dev = usbcontroller.USBController(
select_port(),
baud=current_baud) # Set the current baudrate to communicate
dev.set_baud_rate(baud_rate_setting()) #5: 1Mbps
dev.save_all_registers()
sleep(1)
dev.reboot()
----------------------
モーター接続、各種情報の取得
----------------------
"""
##モーター回転情報callback
def on_motor_measurement_cb(measurement):
print("\033[2;2H\033[2K", end="")
print('measurement {} '.format(measurement), end="", flush=True)
##ログ情報callback
def on_motor_log_cb(log):
print("\033[5;2H\033[2K", end="", flush=True)
print('log {} '.format(log), end="", flush=True)
#接続
dev=usbcontroller.USBController(args.port,False)
dev.on_motor_measurement_value_cb=on_motor_measurement_cb
dev.on_motor_log_cb=on_motor_log_cb
"""
----------------------
モーター動作 絶対位置往復運動
----------------------
"""
is_forward=False
cnt=0
#位置を監視して反転
def direction_measurement_cb(measurement):
on_motor_measurement_cb(measurement)
global is_forward,cnt
import sys
import pathlib
from time import sleep
current_dir = pathlib.Path(__file__).resolve().parent
sys.path.append(str(current_dir) + '/../')
from pykeigan import usbcontroller
from pykeigan import utils
"""
----------------------
モーターを5rpmで 正転(10秒) -> 逆転(10秒) -> 停止(トルクあり) -> 停止(トルク無し)
----------------------
"""
dev = usbcontroller.USBController(
'/dev/ttyUSB0', False) #モーターのアドレス 参照 usb-simple-connection.py
dev.enable_action() #安全装置。初めてモーターを動作させる場合に必ず必要。
dev.set_speed(utils.rpm2rad_per_sec(5)) #rpm -> radian/sec
dev.set_led(1, 0, 200, 0)
dev.run_forward()
sleep(10)
dev.set_led(1, 0, 0, 200)
dev.run_reverse()
sleep(10)
dev.set_led(1, 200, 0, 0)
dev.stop_motor()
##モーター回転情報callback
def on_motor_measurement_cb(measurement):
print("\033[2;2H\033[2K", end="")
print('measurement {} '.format(measurement), end="", flush=True)
##ログ情報callback
def on_motor_log_cb(log):
print("\033[5;2H\033[2K", end="", flush=True)
print('log {} '.format(log), end="", flush=True)
#接続
dev = usbcontroller.USBController('/dev/ttyUSB0', False)
dev.on_motor_measurement_value_cb = on_motor_measurement_cb
dev.on_motor_log_cb = on_motor_log_cb
"""
----------------------
モーター動作 相対移動
----------------------
"""
dev.set_led(2, 255, 255, 0)
sleep(3)
dev.enable_action()
dev.set_speed(utils.rpm2rad_per_sec(10)) #rpm-> rad/sec
dev.move_by_dist(utils.deg2rad(180), None) #Degree-> rad
sleep(5)
dev.move_by_dist(utils.deg2rad(-180), None)
print('speed gain I: ',speedI)
print('speed gain D: ',speedD)
print('position gain P: ',positionP)
print('position gain I: ',positionI)
print('position gain D: ',positionD)
print('position PID threshold [rad]: ', threshold)
print('position PID threshold [deg]: ', utils.rad2deg(threshold))
print('-------')
def read_device_name(motor):
name = motor.read_device_name()
print('Device Name: ', name)
if __name__ == '__main__':
dev = usbcontroller.USBController(select_port())
read_device_name(dev)
read_pid_settings(dev)
try:
while True:
sleep(0.01)
if msvcrt.kbhit():
c = msvcrt.getwch()
print(c)
if c == 'r':
read_pid_settings(dev)
elif c == 'n':
read_device_name(dev)
elif c == 's':
dev.save_all_registers()
if portnum in range(len(portlist)):
portdev = portlist[portnum].device
print('Conncted to', portdev)
return portdev
def baud_rate_setting():
print('Select baud rate')
print('--------')
print('0: 115200')
print('1: 230400')
print('2: 250000')
print('3: 460800')
print('4: 921600')
print('5: 1000000 (1M)')
print('--------')
num = int(input())
while num < 0 or num > 5:
print('Invalid value!')
num = int(input())
return num
port = select_port()
baud_rate = baud_rates[baud_rate_setting()]
dev = usbcontroller.USBController(port, baud=baud_rate)
dev.enable_action() #5: 1Mbps
dev.run_at_velocity(utils.rpm2rad_per_sec(10))
print('imu_measurement {} '.format(measurement), end="", flush=True)
##ログ情報callback
def on_motor_log_cb(log):
print("\033[12;2H\033[2K", end="", flush=True)
print('log {} '.format(log), end="", flush=True)
##エラー情報callback
def on_motor_connection_error_cb(e):
print("\033[16;2H\033[2K", end="", flush=True)
print('error {} '.format(e), end="", flush=True)
#接続
#dev=usbcontroller.USBController('/dev/ttyUSB0',False)#モーターのアドレス 参照 usb-simple-connection.py
dev=usbcontroller.USBController(args.port,False)#モーターのアドレス 参照 usb-simple-connection.py
dev.on_motor_measurement_value_cb=on_motor_measurement_cb
dev.on_motor_imu_measurement_cb=on_motor_imu_measurement_cb
dev.on_motor_log_cb=on_motor_log_cb
dev.on_motor_connection_error_cb=on_motor_connection_error_cb
dev.enable_continual_imu_measurement()#IMUはデフォルトでOFFの為、取得する場合Onにする
#モーター動作
dev.set_led(2,255,255,0)
sleep(3)
dev.enable_action()
dev.set_speed(1.0)
dev.run_forward()
sleep(10)
dev.disable_action()
##ログ情報callback
def on_motor_log_cb(log):
print("\033[12;2H\033[2K", end="", flush=True)
print('log {} '.format(log), end="", flush=True)
##エラー情報callback
def on_motor_connection_error_cb(e):
print("\033[16;2H\033[2K", end="", flush=True)
print('error {} '.format(e), end="", flush=True)
#接続
#dev=usbcontroller.USBController('/dev/ttyUSB0',False)#モーターのアドレス 参照 usb-simple-connection.py
dev = usbcontroller.USBController(
select_port()) #モーターのアドレス 参照 usb-simple-connection.py
dev.on_motor_measurement_value_cb = on_motor_measurement_cb
dev.on_motor_imu_measurement_cb = on_motor_imu_measurement_cb
dev.on_motor_log_cb = on_motor_log_cb
dev.on_motor_connection_error_cb = on_motor_connection_error_cb
dev.enable_continual_imu_measurement() #IMUはデフォルトでOFFの為、取得する場合Onにする
#モーター動作
dev.set_led(2, 255, 255, 0)
sleep(3)
dev.enable_action()
dev.set_speed(1.0)
dev.run_forward()
sleep(10)
dev.disable_action()
