def input_key(m_val,old_rot):
try:
# 直前の入力ログを消去
os.remove('/kaiyo/log/input_log/just_before_read_input_log.txt')
except FileNotFoundError:
pass
CTRL_C = 3
while True:
key = ord(getch())
# 終了
if key == CTRL_C:
stop()
old_rot = move_test(data, 'p', 0, goal_yaw, yaw_MV, goal_depth, depth_MV, set_yaw, set_depth,
old_rot, True)
print("main終了 Crtl+C押して")
break
else:
kb = format(chr(key))
print("test_key", kb)
# ログ再現
if kb == 'S':
send_data("reset rot")
time.sleep(1)
print("どのログを再生する？ 9:大会時 0:直前")
key = ord(getch())
kb = format(chr(key))
print("test_key", kb)
old_rot = move_test(data, kb, m_val, goal_yaw, yaw_MV, goal_depth, depth_MV, set_yaw, set_depth,
old_rot, False)
# モータ出力------------------------
elif kb == 'k': #モータ出力の低下
if m_val > 0:
m_val = m_val - 5;
print(m_val)
elif kb == 'l': #モータ出力の上昇
if m_val <= 60:
m_val = m_val + 5;
print(m_val)
# モータ出力------------------------
else:
# print("1",old_rot)
old_rot = move_test(data, kb, m_val, goal_yaw, yaw_MV, goal_depth, depth_MV, set_yaw, set_depth,
old_rot, True)
# print("2",old_rot)
kb = None
time.sleep(0.2)
def mode_set():
# 念のためモーターstop
stop()
# センサー初期化
send_data("reboot")
# マシンの状態をチェック
status_check(set_lipoC2=7.1, set_lipoC3S3=11.3)
# 待機状態のLEDをセット
led_red()
# textにlogを残すか？
log()
# 動作チェックするか？
# operation_check()
# リードスイッチでスタート
data = get_data("all")
# print data
# スタート動作なし
# while data["mgs"] == 1:
# スタート動作あり
while data["mgs"] == 0:
data = get_data("all")
print data["mgs"]
print "Ready !!"
# センサー初期化
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
# カウントダウン
for cnt in range(3, 0, -1):
led_red()
print cnt
time.sleep(0.5)
led_off()
time.sleep(0.5)
# センサ初期化で起こるずれを
# for i in range(20):
# data = get_data("all")
print "Go !!"
led_yellow()
def f2():
while True:
data = "test2"
print('f2です', data, '\n')
# time.sleep(0.5)
def f3():
while True:
data = g"test3"
print('f3です', data, '\n')
# time.sleep(1)
if __name__ == '__main__':
send_data('reboot')
# データを受信するスレッド
# thread1 = Thread(target=get_read_data)
# thread1.start()
# d1 = Manager().dict()
# d2 = Manager().dict()
# d3 = Manager().dict()
# p0 = Process(target=get_read_data, args=())
p1 = Process(target=f1, args=())
p2 = Process(target=f2, args=())
p3 = Process(target=f3, args=())
# p0.start()
p1.start()
print t10j_map
# up_down(t10j_map[3])
if t10j_map[1] == 0:
# pass
go_back(t10j_map[0])
else:
# pass
spinturn(t10j_map[1])
return 0
# -------------------------------------------------------------------
if __name__ == '__main__':
try:
ini_val = get_data("yaw")
# センサー初期化
send_data("reboot")
while True:
data = get_data("all")
# プロポで操作
t10j(data)
stop()
except KeyboardInterrupt as e:
stop() #Ctrl + Cを押したときの処理
def first_action():
# 設定ファイルから設定やパラメータを読み込む
inifile = ConfigParser.SafeConfigParser()
inifile.read('/kaiyo/my_config/my_config.ini')
set_send_reboot = distutils.util.strtobool(inifile.get('set_mode', 'set_send_reboot'))
set_battery_check = distutils.util.strtobool(inifile.get('set_mode', 'set_battery_check'))
set_sensor_log = distutils.util.strtobool(inifile.get('set_mode', 'set_sensor_log'))
set_gps_log = distutils.util.strtobool(inifile.get('set_mode', 'set_gps_log'))
set_camera = distutils.util.strtobool(inifile.get('set_mode', 'set_camera'))
set_operation_check = distutils.util.strtobool(inifile.get('set_mode', 'set_operation_check'))
set_start_mgs = distutils.util.strtobool(inifile.get('set_mode', 'set_start_mgs'))
set_send_pwm = distutils.util.strtobool(inifile.get('set_mode', 'set_send_pwm'))
set_countdown = int(inifile.get('set_mode', 'set_countdown'))
print
print "----------------------------------"
print "set_send_reboot :", set_send_reboot
print "set_battery_check :", set_battery_check
print "set_sensor_log :", set_sensor_log
print "set_gps_log :", set_gps_log
print "set_camera :", set_camera
print "set_operation_check :", set_operation_check
print "set_start_mgs :", set_operation_check
print "set_send_pwm :", set_send_pwm
print "set_countdown :", set_countdown
print "----------------------------------"
# 念のためモーターstop
stop()
if set_send_reboot:
# センサー初期化
send_data("reboot")
if set_battery_check:
# マシンの状態をチェック
battery_check(set_lipoC2=7.0, set_lipoC3S3=11)
# 待機状態のLEDをセット
led_purple()
if set_operation_check == True:
# 動作チェックするか？
operation_check()
if set_start_mgs:
# リードスイッチでスタート
data = get_data("all")
# スタート動作なし
# while data["mgs"] == 1:
# スタート動作あり
while data["mgs"] == 0:
data = get_data("all")
print data["mgs"]
print "Ready !!"
print "\nPlease wait!!"
# センサー初期化
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
if set_send_pwm:
# プロポ受信モード
send_data("pwm on")
# センサを安定状態にするため
for i in range(20):
data = get_data("all")
if set_sensor_log:
cmd = "python /kaiyo/my_mod/my_data_sampling.py"
subprocess.Popen(cmd.split())
if set_gps_log:
cmd = "python /kaiyo/my_mod/my_gps.py"
subprocess.Popen(cmd.split())
if set_camera:
cmd = "python /kaiyo/my_mod/my_camera.py"
subprocess.Popen(cmd.split())
if set_countdown:
# カウントダウン
for cnt in range(set_countdown, 0, -1):
led_red()
print cnt
time.sleep(0.5)
led_off()
time.sleep(0.5)
print "Go !!"
def motor_tuning():
global xr_cor, xl_cor, yr_cor, yl_cor # 各モータに対応する補正値
flag = [True] * 7 # 一回だけ実行するためのフラグ
fix_flag = True # 出力の強いモータを調べるために使うフラグ
rool = 100 # 回転数
val = 30 # dety比(モータ出力)
eta = 0.02 # 学習率
goal = 0.02 # 目標誤差
t0 = time.perf_counter() # スタート時間
while (True):
if flag[0] == True:
# 値のリセットが完了するまで待機--------------------------------
while True:
stop()
send_data("reboot")
time.sleep(1)
data = get_data("all")
rot0 = data["rot0"]
rot1 = data["rot1"]
rot2 = data["rot2"]
rot3 = data["rot3"]
print(t0, rot0, rot1, rot2, rot3)
if (rot0 == 0 and rot1 == 0 and rot2 == 0 and rot3 == 0):
print("start")
flag[0] = False
start_time = time.perf_counter() # スタート時間
xl(val)
xr(-val)
yl(val)
yr(-val)
break
else:
print("reboot")
# 値のリセットが完了するまで待機-------------------------------
data = get_data("all")
rot0 = data["rot0"]
rot1 = data["rot1"]
rot2 = data["rot2"]
rot3 = data["rot3"]
# 各モータごとに指定の回転回数を超えたか判定-----------------
if rot0 >= rool:
if flag[1] == True:
xr(0)
xr_time = time.perf_counter()
print("rot0_time : ", xr_time - start_time)
flag[1] = False
else:
pass
if rot1 >= rool:
if flag[2] == True:
xl(0)
xl_time = time.perf_counter()
print("rot1_time : ", xl_time - start_time)
flag[2] = False
else:
pass
if rot2 >= rool:
if flag[3] == True:
yl(0)
yl_time = time.perf_counter()
print("rot2_time : ", yl_time - start_time)
flag[3] = False
else:
pass
if rot3 >= rool:
if flag[4] == True:
yr(0)
yr_time = time.perf_counter()
print("rot3_time : ", yr_time - start_time)
flag[4] = False
else:
pass
# 各モータごとに指定の回転回数を超えたか判定-----------------
# すべてのモータ回転終了
if (rot0 >= rool and rot1 >= rool and rot2 >= rool and rot3 >= rool):
print(rot0, rot1, rot2, rot3)
print("time_dif go: ", XR_time - t3)
print("time_dif up: ", yl_time - yr_time)
# モータをひとつ固定してもう一方を調整(今のままだと補正値1.0超える)----
if (XR_time - xl_time) >= goal: # 0.02以上
xr_cor += eta
elif (XR_time - xl_time) <= -goal: # -0.02以下
xr_cor -= eta
else:
print("go_back_OK")
flag[5] = False
if (yl_time - yr_time) >= goal:
yl_cor += eta
elif (yl_time - yr_time) <= -goal:
yl_cor -= eta
else:
print("up_down_OK")
flag[6] = False
# モータをひとつ固定してもう一方を調整(今のままだと補正値1.0超える)----
print(xr_cor, xl_cor, yl_cor, yr_cor)
# モータ終了時間の差が指定時間(0.02)以内
if (flag[5] == False and flag[6] == False):
print("end")
break
else: #もう一度
flag = [True] * 7
t6 = time.perf_counter()
print("all_time : ", t6 - t0)
ini_set('set_mode', 'rot0_cor', str(xr_cor))
ini_set('set_mode', 'rot1_cor', str(xl_cor))
ini_set('set_mode', 'rot2_cor', str(yl_cor))
ini_set('set_mode', 'rot3_cor', str(yr_cor))
print(xr_cor, xl_cor, yl_cor, yr_cor)
return True
def first_action(sensordata):
# 設定ファイルから設定やパラメータを読み込む
inifile = configparser.SafeConfigParser()
inifile.read('/kaiyo/my_config/my_config.ini')
set_send_reboot = distutils.util.strtobool(
inifile.get('set_mode', 'set_send_reboot'))
set_battery_check = distutils.util.strtobool(
inifile.get('set_mode', 'set_battery_check'))
set_sensor_log = distutils.util.strtobool(
inifile.get('set_mode', 'set_sensor_log'))
set_gps_log = distutils.util.strtobool(
inifile.get('set_mode', 'set_gps_log'))
set_camera = distutils.util.strtobool(inifile.get('set_mode',
'set_camera'))
set_operation_check = distutils.util.strtobool(
inifile.get('set_mode', 'set_operation_check'))
set_move_test = distutils.util.strtobool(
inifile.get('set_mode', 'set_move_test'))
set_start_mgs = distutils.util.strtobool(
inifile.get('set_mode', 'set_start_mgs'))
set_send_pwm = distutils.util.strtobool(
inifile.get('set_mode', 'set_send_pwm'))
set_countdown = int(inifile.get('set_mode', 'set_countdown'))
print()
print("----------------------------------")
print("set_send_reboot :", set_send_reboot)
print("set_battery_check :", set_battery_check)
print("set_sensor_log :", set_sensor_log)
print("set_gps_log :", set_gps_log)
print("set_camera :", set_camera)
print("set_operation_check :", set_operation_check)
print("set_start_mgs :", set_start_mgs)
print("set_send_pwm :", set_send_pwm)
print("set_countdown :", set_countdown)
print("set_move_test :", set_move_test)
print("----------------------------------")
# 念のためモーターstop
stop()
if set_send_reboot:
# センサー初期化
send_data("reboot")
if set_battery_check:
# マシンの状態をチェック
battery_check(set_lipoC2=7.0, set_lipoC3S3=11)
# 待機状態のLEDをセット
led_purple()
if set_operation_check:
# 動作チェックするか？
operation_check()
if set_start_mgs:
# リードスイッチでスタート
# data = get_data("all")
# スタート動作なし
# while data["mgs"] == 1:
# スタート動作あり
while data["mgs"] == 0:
data = get_data("all")
print(data["mgs"])
print("Ready !!")
print("\nPlease wait!!")
# センサー初期化
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
send_data("reboot")
time.sleep(0.5)
if set_send_pwm:
# プロポ受信モード
send_data("pwm on")
# センサを安定状態にするため
for i in range(20):
data = get_data("all")
if set_sensor_log:
set_sample_rate = 0.2
sensor_process = Process(target=data_sampling,
args=[data, set_sample_rate])
sensor_process.start()
#
# if set_gps_log:
# gps_process = Process(target=get_gps_data, args=())
# gps_process.start()
#
# if set_camera:
# camera_process = Process(target=camera2)
# camera_process.start()
if set_countdown:
# カウントダウン
for cnt in range(set_countdown, 0, -1):
led_red()
print(cnt)
time.sleep(0.5)
led_off()
time.sleep(0.5)
print("Go !!")
def move_test(data,
kb,
m_val,
goal_yaw,
yaw_MV,
goal_depth,
depth_MV,
set_yaw,
set_depth,
old_rot=[0] * 6,
set_input_kb=True):
# m_val = 30
# CTRL_C = 3
# 大会用のログ
path1 = '/kaiyo/log/input_log/read_input_log.txt'
# 直前の入力ログ 次回実行時に消える
path2 = '/kaiyo/log/input_log/just_before_read_input_log.txt'
# ティーチング用のログを作る-----------------------------------------
if set_input_kb == True:
# 今日の日付でフォルダを作る
try:
os.makedirs('/kaiyo/log/input_log/' +
str(datetime.now().strftime('%Y%m%d')))
except FileExistsError:
pass
# input_log_file_make = open('/kaiyo/log/input_log/'+str(datetime.now().strftime('%Y%m%d'))+'/input_log_'+str(datetime.now().strftime('%Y%m%d_%H%M%S'))+'.txt', 'a')
input_log_file_make = open(
'/kaiyo/log/input_log/' + str(datetime.now().strftime('%Y%m%d')) +
'/input_log_.txt', 'a')
just_before_input_log_file = open(
'/kaiyo/log/input_log/just_before_read_input_log.txt', 'a')
# ティーチング用のログを作る-----------------------------------------
# ティーチング用のログを読込む---------------------------------------
# elif set_input_kb == False:
if set_input_kb == False:
try:
if kb == '9':
path = path1
if kb == '0':
path = path2
with open(path) as f:
# l_strip = [s.strip() for s in f.readlines()]
# input_log_file_read = ast.literal_eval(l_strip[1])
input_log_file_read = [s.strip() for s in f.readlines()]
fr_len = len(input_log_file_read)
except FileExistsError:
print("file_error")
pass
else:
print("move start")
# ティーチング用のログを読込む---------------------------------------
line_num = 0
now_rot = 0
next_rot = 0
data_rot = 0
old_data_rot = 0
s_flg = 0
while True:
try:
# ログのから１行ずつ処理
if set_input_kb == False:
# print(line_num)
now_line = ast.literal_eval(input_log_file_read[line_num])
# リストがあるなら
# rot4, 5を使っているのはロータリーが4つしか取れなかったから配線を入れ替えたため
if line_num + 1 < fr_len:
next_line = ast.literal_eval(input_log_file_read[line_num +
1])
next_rot = (next_line["rot0"] + next_line["rot1"] +
next_line["rot4"] + next_line["rot5"]) / 4
data_rot = (data["rot0"] + data["rot1"] + data["rot4"] +
data["rot5"]) / 4
now_rot = (now_line["rot0"] + now_line["rot1"] +
now_line["rot4"] + now_line["rot5"]) / 4
print("line_num", line_num, "next_key:", now_line['input_key'],
"kb:", kb, "data:", data_rot, "now:", now_rot, "next:",
next_rot, "差:", (old_data_rot - next_rot), "残:",
(now_rot - (data_rot - old_data_rot)), "\n")
if line_num == 0:
line_num = line_num + 1
kb = str(now_line['input_key'])
m_val = now_line['m_val']
goal_yaw.value = now_line['goal_yaw']
goal_depth.value = now_line['goal_depth']
set_yaw.value = now_line['set_yaw']
set_depth.value = now_line['set_depth']
old_data_rot = data_rot
print("test,test0", set_yaw, set_depth)
else:
# ロータリが指定回数以上回った か スラスタが回らないコマンドだったら 次の行を読み込む
if (((now_rot - (data_rot - old_data_rot)) <= 0)
or (kb == 's') or s_flg
== 1): # and ((kb != 'A') or (kb != 'D')):
s_flg = 0
line_num = line_num + 1
# 各値の更新
kb = str(now_line['input_key'])
m_val = now_line['m_val']
goal_yaw.value = now_line['goal_yaw']
goal_depth.value = now_line['goal_depth']
set_yaw.value = now_line['set_yaw']
set_depth.value = now_line['set_depth']
old_data_rot = data_rot
print("data_rot", old_data_rot)
print("test, test1", set_yaw, set_depth)
# キー入力のログを残す
if set_input_kb == True:
# input_log_file_make.write("{'time':" + str(datetime.now().strftime('%M:%S.%f')) +", 'inputkey':" + str(kb) + ", 'moterval':" + str(m_val) + "}\n")
# end_time = time.time()
# 日付ごとに入力したログ用
input_log_file_make.write(
"{'time':" + str(data["time"]) + ", 'input_key':'" +
str(kb) + "'" + ", 'm_val':" + str(m_val) +
", 'goal_yaw':" + str(goal_yaw.value) +
", 'goal_depth':" + str(goal_depth.value) +
", 'set_yaw':" + str(set_yaw.value) + ", 'set_depth':" +
str(set_depth.value) + ", 'rot0':" +
str(data["rot0"] - old_rot[0]) + ", 'rot1':" +
str(data["rot1"] - old_rot[1]) + ", 'rot2':" +
str(data["rot2"] - old_rot[2]) + ", 'rot3':" +
str(data["rot3"] - old_rot[3]) + ", 'rot4':" +
str(data["rot4"] - old_rot[4]) + ", 'rot5':" +
str(data["rot5"] - old_rot[5]) + "}\n")
# 直前の動作用
just_before_input_log_file.write(
"{'time':" + str(data["time"]) + ", 'input_key':'" +
str(kb) + "'" + ", 'm_val':" + str(m_val) +
", 'goal_yaw':" + str(goal_yaw.value) +
", 'goal_depth':" + str(goal_depth.value) +
", 'set_yaw':" + str(set_yaw.value) + ", 'set_depth':" +
str(set_depth.value) + ", 'rot0':" +
str(data["rot0"] - old_rot[0]) + ", 'rot1':" +
str(data["rot1"] - old_rot[1]) + ", 'rot2':" +
str(data["rot2"] - old_rot[2]) + ", 'rot3':" +
str(data["rot3"] - old_rot[3]) + ", 'rot4':" +
str(data["rot4"] - old_rot[4]) + ", 'rot5':" +
str(data["rot5"] - old_rot[5]) + "}\n")
old_rot[0] = data["rot0"]
old_rot[1] = data["rot1"]
old_rot[2] = data["rot2"]
old_rot[3] = data["rot3"]
old_rot[4] = data["rot4"]
old_rot[5] = data["rot5"]
# モータ一個ずつ動かす------------------------
if kb == '1': #xlf
xlf(m_val)
print("xlf:", m_val)
if kb == '2': #xrf
xrf(-m_val)
print("xrf:", m_val)
if kb == '3': #xl
xlb(m_val)
print("xlb:", m_val)
if kb == '4': #xr
xrb(-m_val)
print("xrb:", m_val)
if kb == '5': #yl
yl(m_val)
print("yl:", m_val)
if kb == '6': #yr
yr(-m_val)
print("yr:", m_val)
# モータ一個ずつ動かす------------------------
# 航行---------------------------------------
if kb == 'w': #前進
go_back(m_val)
print("前進:", m_val)
if kb == 'W': #前進
if set_yaw.value == True:
set_yaw.value = False
stop()
print("制御前進:end", set_yaw.value)
s_flg = 1
else:
set_yaw.value = True
print("制御前進:end_key:W", set_yaw.value)
# s_flg = 1
if set_yaw.value == True:
if (yaw_MV.value >= 0):
go_back_each(m_val + yaw_MV.value, m_val - yaw_MV.value,
m_val + yaw_MV.value, m_val - yaw_MV.value)
elif (yaw_MV.value < 0):
go_back_each(m_val + yaw_MV.value, m_val - yaw_MV.value,
m_val + yaw_MV.value, m_val - yaw_MV.value)
if set_yaw.value == True:
if (yaw_MV.value >= 0):
go_back_each(m_val + yaw_MV.value, m_val - yaw_MV.value,
m_val + yaw_MV.value, m_val - yaw_MV.value)
elif (yaw_MV.value < 0):
go_back_each(m_val + yaw_MV.value, m_val - yaw_MV.value,
m_val + yaw_MV.value, m_val - yaw_MV.value)
if kb == 'x': #後退
go_back(-m_val)
print("後退:", m_val)
if kb == 'q': #潜水
up_down(-m_val)
print("潜水:", m_val)
if kb == 'X': #制御潜水
if set_depth.value == True:
set_depth.value = False
stop()
print("制御潜水:end")
else:
set_depth.value = True
print("制御潜水:end_key:X")
if set_depth.value == True: #前進
print("depth_MV.value:", depth_MV.value)
up_down(depth_MV.value)
print("制御前進:", m_val)
if kb == 'e': #上昇
up_down(m_val)
print("上昇:", m_val)
if kb == 'z': #左傾き
roll(m_val)
print("左傾き:", m_val)
if kb == 'c': #右傾き
roll(-m_val)
print("右傾き:", m_val)
if kb == 'a': #左旋回
spinturn(m_val)
print("右旋回:", m_val)
if kb == 'd': #右旋回
spinturn(-m_val)
print("左旋回:", m_val)
if (kb == 'A') or (kb == 'D'): #180旋回
set_yaw.value = False
goal_yaw.value = goal_yaw.value + 180
if goal_yaw.value >= 360:
goal_yaw.value = 0
# ある程度旋回させる (180度超えないぐらいがいいと思う)
if (kb == 'A'):
spin_val = 60
if (kb == 'D'):
spin_val = -60
spinturn(spin_val)
time.sleep(3.5)
# 回転させたときの誤差を修正
while True:
try:
print("goal", goal_yaw.value, "com:", data["compass"],
"yaw", (abs(data["yaw"]) - 10),
(abs(data["yaw"])), (abs(data["yaw"] + 10)),
"180度旋回:", yaw_MV.value)
yaw_val = 10 * yaw_MV.value
yaw_val = min(60, yaw_val)
# if (yaw_val >= 60):
# yaw_val = 60
spinturn(yaw_MV.value)
if ((data["yaw"] - 10) < goal_yaw.value < (data["yaw"] + 10)) or \
((data["yaw"] - 10) > goal_yaw.value > (abs(data["yaw"] + 10))):
break
time.sleep(0.2)
except KeyboardInterrupt as e:
# Ctrl-cを押したときの処理
print("\n-----------")
print("move.py_180trun : ", e)
print("-----------\n")
# line_num = line_num + 1
s_flg = 1
print("spin_end")
stop_go_back()
if kb == 'E': #右前
xlf(m_val * 1.3)
xrb(-m_val)
print("右前:", m_val)
if kb == 'Q': #左前
xrf(-m_val * 1.3)
xlb(m_val)
print("左前:", m_val)
if kb == 'C': #右後
xlf(-m_val * 1.3)
xrb(m_val)
print("右後:", m_val)
if kb == 'Z': #左後
xrf(m_val * 1.3)
xlb(-m_val)
print("左後:", m_val)
# 航行---------------------------------------
# 停止---------------------------------------
if kb == 's': #全停止
stop()
print("全停止:")
if kb == 'g': #推進用停止
stop_go_back()
print("推進用停止:")
if kb == 'v': #潜水用停止
stop_up_down()
print("潜水用停止:")
# 停止---------------------------------------
# オプション---------------------------------------
if kb == 'i': #データ表示
print(data)
if kb == 'R': #データ表示
old_rot[0] = 0
old_rot[1] = 0
old_rot[2] = 0
old_rot[3] = 0
old_rot[4] = 0
old_rot[5] = 0
while True:
try:
print("send:reboot")
send_data("reboot")
time.sleep(1)
if data["time"] <= 10.0:
# time.sleep(1)
print("OK\n Start")
break
except TimeoutError:
print("main:timeout Error!\n")
except SyntaxError:
# 受信エラー
print("main : Reception Error!!\n")
# yawの目標値を変更する
if kb == 'y':
print("g_now:", goal_yaw.value, "y_now", data["yaw"])
in_str = input("goal_yaw.val=")
goal_yaw.value = int(in_str)
print(goal_yaw.value)
if kb == 't':
print("now", goal_depth.value)
in_str = input("goal_depth.val=")
goal_depth.value = float(in_str)
print(goal_depth.value)
if kb == 'f': #arduinoにコマンド送信
in_str = input("command>>>")
send_data(in_str)
if kb == 'h':
print("'run':'シリアル通信を開始する。', ")
print("'stop':'シリアル通信を停止する。', ")
print("'reboot':'Arduinoを再起動する。', ")
print("'reset xxx':'curまたはrotの値をリセットする。', ")
print("'debug on/off':'デバッグモードに移行/通常モードに戻る。', ")
print("'time XXXX':'無限ループ時の待機時間をXXXXミリ秒にする。', ")
print("'yaw_zero off':'yawの初期リセット値を無効化', ")
print("'remove error':'状態を確認し問題なかったらstateをnormalにする', ")
print("'puropo on/off':'プロポを有効/無効にする', ")
if kb == 'b': #指定時間潜水
up_down(m_val)
print("10秒潜水:", m_val)
time.sleep(10)
stop_up_down()
if kb == 'n': #指定時間浮上
up_down(-m_val)
print("3秒浮上:", m_val)
time.sleep(3)
stop_up_down()
s_flg = 1
print("3秒浮上終了")
if kb == 'p': #終了
# my_exit()
stop()
print("終了します")
return old_rot
# オプション---------------------------------------
# 意味なし
if set_input_kb == True:
if (kb == "w") or (kb == "W") or (kb == "s") or (
kb == "a") or (kb == "A") or (kb == "d") or (
kb == "D") or (kb == "x") or (kb
== "X") or (kb
== "p"):
return old_rot
else:
return old_rot
# break
# ティーチングのとき使うかもしれないからリセット
if (kb != "W") or (kb != "X") or (kb != 's'):
kb = None
time.sleep(0.2)
# print(kb)
except KeyboardInterrupt as e:
stop()
prtin("move:Keyerror")
break