def move_straight(self, rpm, degree, timeout = 0): # 左右のモーターを前進するように、degree 角度分だけ回転させる。timeout[s]を超えると安全装置再開
self.safe_setting(False) # 安全装置解除
self.left.set_speed(utils.rpm2rad_per_sec(rpm))
self.right.set_speed(utils.rpm2rad_per_sec(rpm))
self.left.move_by_dist(utils.deg2rad(degree))
self.right.move_by_dist(utils.deg2rad(-degree))
# timeout == 0 の場合、安全装置は解除されたまま、動作完了まで続ける
if timeout > 0:
time.sleep(timeout)
self.safe_setting(True) # 安全装置再開
def pivot_turn(self, rpm, degree, timeout = 0):
self.safe_setting(False) # 安全装置解除
self.left.set_speed(utils.rpm2rad_per_sec(rpm))
self.right.set_speed(utils.rpm2rad_per_sec(rpm))
dist = 2 * math.pi * (self.round_num * degree / 360) # 2π * (真上から見て一回転する距離 * 角度/360)
self.left.move_by_dist(-dist)
self.right.move_by_dist(-dist)
# timeout == 0 の場合、安全装置は解除されたまま、動作完了まで続ける
if timeout > 0:
time.sleep(timeout)
self.safe_setting(True) # 安全装置再開
def start_torque_control_like_closing_cap():
global torque_level
print('\033[2;1H\033[2K', 'Please try to turn the motor by hand.', end="", flush=True)
dev.disable_action()
dev.preset_position(0)
sleep(0.2)
dev.enable_action()
dev.move_to_pos(0,utils.rpm2rad_per_sec(10))
torque_level=10
dev.on_motor_measurement_value_cb = on_motor_measurement_cb
def v_operator(self):
# print("sleep started")
self.dev.set_speed(utils.rpm2rad_per_sec(30))
self.dev.move_to_pos(0.0)
stop = False
while self.p["sleep"] <= 90:
# @お家に変える
# print("\033[" + str(24) + ";2H\033[2K" + "sleeping", end="")
# print("sleeping")
self.p["sleep"] = self.p["sleep"] + 0.1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
print("sleep cancelled")
break
if not stop:
# print("sleep finished")
self.finisher()
# print("\033[" + str(24) + ";2H\033[2K" + "", end="")
return
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()
is_forward=False
cnt += 1
if cnt > 4:
dev.disable_action()
else:
dev.set_led(2, 255, 0, 255)
dev.move_to_pos(utils.deg2rad(0))
if not is_forward and measurement['position']<(0+0.02):
is_forward=True
dev.set_led(2, 0, 255, 255)
dev.move_to_pos(utils.deg2rad(720)) # 2回転
dev.set_speed(utils.rpm2rad_per_sec(20))
dev.preset_position(0)#現在位置の座標を0に設定
dev.enable_action()
dev.on_motor_measurement_value_cb=direction_measurement_cb
"""
Exit with key input
"""
sleep(0.5)
while True:
print("\033[10;2H", end="",flush=True)
print("---------------------------------------")
inp = input('Exit:[key input] >>')
if inp !=None:
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()
sleep(10)
if ready:
return sys.stdin.readline().rstrip('\n')
else:
return 0
current_dir = pathlib.Path(__file__).resolve().parent
sys.path.insert(0, str(current_dir) + '/../') # give 1st priority to the directory where pykeigan exists
from time import sleep
from pykeigan import blecontroller
from pykeigan import utils
dev=blecontroller.BLEController("fe:e1:8c:0a:7d:a0")#下モータ
dev2=blecontroller.BLEController("ef:23:f5:42:8f:be")#上モータ
speed = utils.rpm2rad_per_sec(10)
dev.enable_action()
dev.set_led(1, 0, 200, 0)
dev.set_speed(speed)
dev.move_to_pos(0.3)
sleep(3)
dev2.enable_action()
dev2.set_led(1, 0, 200, 0)
dev2.set_speed(speed)
dev2.set_max_torque(100)
dev2.move_to_pos(-2.9)
sleep(3)
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)
sleep(5)
dev.move_by_dist(utils.deg2rad(360), utils.rpm2rad_per_sec(15)) #rpm-> rad/sec
sleep(6)
"""
----------------------
モーター動作 絶対位置移動
----------------------
"""
dev.set_curve_type(1)
dev.set_led(2, 0, 255, 255)
dev.set_speed(utils.rpm2rad_per_sec(30))
def runner(self):
active_layer = [-1, "none"]
loop = asyncio.get_event_loop()
while True:
# print("working")
temp_active_layer = active_layer
self.sense()# active_layer, temp_active_layer) # 感覚入力を更新
# 活性化しているモジュールを取得 (活性化はこの処理で、非活性化はもモジュールごとのv_operatorで行う(非可逆な基準))
for i in range(len(self.modules)):
if active_layer[0] >= 0 and i >= active_layer[0]:
break # 自分より下位のレイヤーしか見ない(下位のレイヤーがアクティブになった場合はそちらが優先権を取る)
temp_key = list(self.modules.keys())[i]
# self.print_status(19, temp_key + " lookat | " + str(active_layer[0]) + "\n")
if self.modules[temp_key].is_active(self.physio):
self.print_status(10, temp_key + " activated" + "\n")
# print(temp_key + " activated")
active_layer = [i, temp_key]
break
if active_layer != temp_active_layer:
# モジュールをスタートさせる
# print("kill working module : " + temp_active_layer[1])
self.print_status(10, "subsuming working module : " + temp_active_layer[1] + ", ")
if temp_active_layer[0] >= 0:
self.modules[temp_active_layer[1]].stopper = True
self.print_status(10, '\033[30m' + "active module : " + '\033[0m' + active_layer[1] + "\n")
# print("module start : " + active_layer[1])
self.modules[active_layer[1]].s = self.sensor_info
self.modules[active_layer[1]].p = self.physio
self.modules[active_layer[1]].b = self.belief
self.modules[active_layer[1]].waiting = True
if temp_active_layer[0] >= 0:
# モジュールを終了させる
if self.modules[active_layer[1]].finished:
self.print_status(40, "module kill" + "\n")
self.modules[active_layer[1]].finished = False
active_layer = [-1, "none"]
# @noneになったらとりあえず巣に帰らせる
self.dev.set_speed(utils.rpm2rad_per_sec(10))
self.dev.move_to_pos(0.0)
self.print_status(10, '\033[30m' + "active module : " + '\033[0m' + active_layer[1] + "\n")
self.physio_sim() # 生理状態のシミュレーション
self.show_physio()
self.print_status(30, str(active_layer) + "\n")
try:
self.action_log[active_layer[1]] += 0.05
except KeyError:
pass
try:
time.sleep(0.05)
except KeyboardInterrupt:
self.dev.disable_action()
self.dev.disconnect()
def my_taskset():
dev.set_speed(utils.rpm2rad_per_sec(5))
dev.set_led(1, 0, 200, 0)
dev.move_by_dist(utils.deg2rad(30))
dev.wait_queue(2000) # wait for 5000 ms
dev.stop_motor()
# ログ情報callback
def on_motor_log_cb(log):
print('log {} '.format(log))
if log['error_codes'] == 'KM_SUCCESS':
print('Command Success')
elif log['error_codes'] == 'KM_SUCCESS_ARRIVAL':
print('Position Arrival Success')
# モーター回転情報callback
def on_motor_measurement_cb(measurement):
print("\r" + 'measurement {} '.format(measurement), end="")
dev = usbcontroller.USBController(select_port())
dev.on_motor_log_cb = on_motor_log_cb
dev.on_motor_measurement_value_cb = on_motor_measurement_cb
dev.enable_action()
dev.set_curve_type(0)
dev.set_speed_p(10)
dev.set_safe_run_settings(
True, 100, 1
) # 第1引数が True の場合、5000[ms]以内に次の動作命令が来ないと、停止する 0:free,1:disable,2:stop, 3:position固定
dev.run_at_velocity(utils.rpm2rad_per_sec(20))
"""
Exit with key input
"""
while True:
dev.run_at_velocity(utils.rpm2rad_per_sec(20))
sleep(0.01)
def run(self, left_rpm, right_rpm): # 左右のモーターを指定の速度rpmで動作させる
self.left.run_at_velocity(utils.rpm2rad_per_sec(left_rpm))
self.right.run_at_velocity(utils.rpm2rad_per_sec(-right_rpm))
