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))