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 v_operator(self):
print("observation started")
stop = False
self.dev.set_max_torque(0.03)
self.dev.move_to_pos(utils.deg2rad(-30), (utils.deg2rad(90) / 3))
t = 0
while t <= 30:
t += 1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
print("observation cancelled")
break
self.dev.move_to_pos(0.01)
t = 0
while t <= 20:
t += 1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
print("observation cancelled")
break
if not stop:
# self.p["obs"] = 100
print("observation finished")
self.finisher()
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 direction_measurement_cb(measurement):
on_motor_measurement_cb(measurement)
global is_forward,cnt
if is_forward and measurement['position'] > (utils.deg2rad(720) - 0.02):
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回転
def v_operator(self):
print("reflection started")
if self.fight_flight() == "fight":
temp = -self.s["pos"]
self.dev.set_speed(utils.deg2rad(840))
self.dev.set_curve_type(2)
self.dev.set_acc(200)
self.dev.set_max_torque(0.5)
self.dev.move_to_pos(utils.deg2rad(min(temp + 20, 0)))
time.sleep(0.2)
self.dev.move_to_pos(utils.deg2rad(temp - 10))
time.sleep(1)
pass
# @暴れる
else:
print("\033[" + str(39) + ";2H\033[2K" + "flight", end="")
self.dev.set_speed(utils.deg2rad(840))
self.dev.set_curve_type(2)
self.dev.set_acc(200)
self.dev.set_max_torque(0.5)
self.dev.move_to_pos(utils.deg2rad(min(self.s["pos"] + 40, 0)))
time.sleep(2)
print("finished")
self.finisher()
self.dev.set_max_torque(0.03)
self.dev.set_speed(utils.deg2rad(180))
self.dev.set_curve_type(1)
self.dev.set_acc(100)
def scheduler(arg1, args2):
global ampDegree, t, res, oneRound
target = utils.deg2rad(ampDegree * math.sin(t * 2 * math.pi / oneRound))
dev.move_to_pos(target)
t += 1
dev.on_motor_measurement_value_cb = on_motor_measurement_cb
dev.enable_action()
dev.set_speed(utils.rpm2rad_per_sec(10))
dev.reset_all_pid()
dev.move_to_pos(0)
sleep(2)
print("sine wave start !")
# 目標位置からの偏差には以下のゲインを調整するか、振幅自体を変える
# 振幅自体変えた方がベター??
# 新設 curveType:10 ダイレクト位置制御（速度制限、カーブなしでマスター側が時間管理して位置制御を行う。）
dev.set_curve_type(10)
dev.set_speed_p(3) # デフォルト 14 振動対策
dev.set_speed_i(0)
dev.set_pos_control_threshold(utils.deg2rad(1000))
dev.set_position_i(80)
# # シグナルライブラリ（schedular関数を定期的に呼ぶ, timer_start(), timer_stop()も参照）で定期実行することの宣言
signal.signal(signal.SIGALRM, scheduler)
timer_start()
try:
while True:
continue
except KeyboardInterrupt:
if dev:
dev.disable_action()
print('Ctrl-C')
def v_operator(self):
# print("interact started")
stop = False
accumulative = 0
t = 0
temp = 0
def_pos = 0
while self.p["play"] <= 90:
t += 1 # このtでランダムな移動を作る
if t >= temp:
t = 0
temp = random.randint(20, 60)
if self.p["sleep"] <= 50:
def_pos = random.randint(35, 55)
else:
def_pos = random.randint(45, 65)
# self.dev.set_speed(utils.rpm2rad_per_sec(30))
self.dev.set_speed(utils.deg2rad(40))
self.dev.move_to_pos(utils.deg2rad(-def_pos))
# 報酬の処理
self.p["play"] = self.p["play"] + 0.01 # 0.06
if np.var(np.array(self.s["touch"])[:, 0]) + np.var(
np.array(self.s["touch"])[:, 1]) + np.var(
np.array(self.s["touch"])[:, 2]) >= 2000:
accumulative += 1
else:
accumulative -= 0.1
if accumulative <= 0:
accumulative = 0
if accumulative >= 22:
accumulative = 0
self.p["play"] = self.p["play"] + 15
self.dev.set_speed(utils.deg2rad(640))
for i in range(5):
self.dev.move_to_pos(utils.deg2rad(-def_pos))
time.sleep(0.18)
self.dev.move_to_pos(utils.deg2rad(-def_pos + 20))
time.sleep(0.18)
print("\033[" + str(39) + ";2H\033[2K" +
"=================== touch " + str(accumulative) + "====" +
str(np.var(np.array(self.s["touch"])[:, 0])) + "|" +
str(np.var(np.array(self.s["touch"])[:, 1])) + "|" +
str(np.var(np.array(self.s["touch"])[:, 2])),
end="")
# 中断の処理
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
print("play cancelled")
break
print("\033[" + str(24) + ";2H\033[2K" + "playing score: " +
str(accumulative),
end="")
if not stop:
print("play finished")
self.finisher()
return
#接続
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))
dev.preset_position(0) #現在位置の座標を0に設定
dev.move_to_pos(utils.deg2rad(90), (utils.deg2rad(90) / 3))
up.set_speed(0.5) #radian/sec
down.set_speed(0.5) #radian/sec
up.set_acc(0.5) #rpm -> radian/sec
down.set_acc(0.5) #rpm -> radian/sec
up.set_max_torque(0.1)
down.set_max_torque(0.1)
up.set_led(1, 0, 200, 0)
down.set_led(1, 0, 200, 0)
up_min_deg = -45
up_max_deg = 90
up_min_pos = utils.deg2rad(up_min_deg) + up_init_pos
up_max_pos = utils.deg2rad(up_max_deg) + up_init_pos
print("up init pos:", up_init_pos)
print("up range:", up_min_pos, up_max_pos)
down_min_deg = -120
down_max_deg = 120
down_min_pos = utils.deg2rad(down_min_deg) + down_init_pos
down_max_pos = utils.deg2rad(down_max_deg) + down_init_pos
print("down init pos:", down_init_pos)
print("down range:", down_min_pos, down_max_pos)
def v_operator(self):
# print("\033[" + str(50) + ";2H\033[2K" + "eat module in action", end="")
stop = False
t = 0
def_pos = 10
self.dev.set_max_torque(0.03)
while self.p["eat"] <= 80:
if self.p["sleep"] <= 50:
def_pos = random.randint(0, 10)
else:
def_pos = random.randint(35, 55)
# self.dev.set_speed(utils.rpm2rad_per_sec(30))
self.dev.set_speed(utils.deg2rad(180))
self.dev.move_to_pos(utils.deg2rad(-def_pos))
temp = random.randint(30, 60)
t = 0
while t <= temp:
t += 1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
break
if stop:
print("eat cancelled on waiting")
break
if random.choice([False, False, True]):
def_pos = 0
self.dev.move_to_pos(utils.deg2rad(-def_pos))
temp = random.randint(10, 40)
t = 0
while t <= temp:
t += 1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
break
if stop:
print("eat cancelled on waiting")
break
is_flying = False
if self.b["food"]:
if self.p["sleep"] <= 50:
self.dev.set_speed(utils.deg2rad(random.randint(20, 50)))
else:
self.dev.set_speed(utils.deg2rad(840))
self.dev.set_curve_type(2)
self.dev.set_acc(200)
self.dev.set_max_torque(0.4)
self.dev.move_to_pos(utils.deg2rad(-130)) # 押しに行く
is_flying = True
if is_flying:
while True:
if self.stopper:
self.stopper = False
stop = True
break
if not self.s["food"]:
if self.s["pos"] >= 85:
self.p["eat"] = self.p["eat"] + 30
break
if self.stopper or stop: # 食事を邪魔された場合はループから抜ける
self.stopper = False
stop = True
print("eat cancelled on flight")
self.dev.set_max_torque(0.03)
self.dev.set_speed(utils.deg2rad(180))
self.dev.set_curve_type(1)
self.dev.set_acc(100)
return
if is_flying: # 食った後は戻る
self.dev.move_to_pos(utils.deg2rad(-def_pos))
temp = random.randint(30, 60)
t = 0
while t <= temp:
t += 1
time.sleep(0.1)
if self.stopper:
self.stopper = False
stop = True
break
if stop:
print("eat cancelled on waiting")
break
time.sleep(0.1)
if self.stopper or stop:
self.stopper = False
stop = True
print("eat cancelled on back")
if not stop:
print("eat finished")
self.finisher()
# print("\033[" + str(50) + ";2H\033[2K" + "snapped", end="")
self.dev.set_max_torque(0.03)
self.dev.set_speed(utils.deg2rad(180))
self.dev.set_curve_type(1)
self.dev.set_acc(100)
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()
#dev.set_position_i(2)
#dev.set_speed(utils.rpm2rad_per_sec(20))
# 連続で動作命令を送る場合、位置到達時の通知設定をOFFとする必要がある
# dev.set_notify_pos_arrival_settings(False, 0.00872665, 200) # 第1引数 False で無効化
dev.set_safe_run_settings(
True, 100, 3
) # 第1引数が True の場合、5000[ms]以内に次の動作命令が来ないと、停止する 0:free,1:disable,2:stop, 3:position固定
# 最初 0[deg] へ移動
targetPos = 0
inc = 0
"""
Exit with key input
"""
try:
while True:
dev.move_to_pos(utils.deg2rad(targetPos + inc))
inc += 0.1 # 少しずつ目標位置をずらしていく。位置固定の場合はこの行をコメントアウト
sleep(0.01)
if msvcrt.kbhit():
c = msvcrt.getwch()
print(c)
if c == 'f': # 1080[deg] へ移動
targetPos = 1080
elif c == 'b': # 0[deg] へ移動
targetPos = 0
except KeyboardInterrupt:
if dev:
dev.disable_action()
print('Ctrl-C')
