def go_yaw_onoff_kaeri(set_speed, set_rot, set_diving=True):
set_rot_old = get_data("rot0")
while True:
if set_diving:
diving(set_diving)
# yawを取得( 180 ~ 0 ~ -180)
now_val = get_data("yaw2")
print now_val
# この範囲の時は直進
if now_val >= 179 or now_val <= -179:
r = set_speed
l = set_speed
else:
if now_val <= 0:
# 右に動かす
r = int(set_speed / 1.5)
l = set_speed
else:
# 左に動かす
r = set_speed
l =int(set_speed / 1.5)
go_back_each(l, r)
print l, r
now_rot0 = get_data("rot0")
print "rot---------------------------------",now_rot0 - set_rot_old
print
if now_rot0 - set_rot_old >= set_rot:
stop()
break
def pad_rc_route_data_creation():
gps_route_data = open('/kaiyo/log/gsp_route_data/gps_route_data.txt', 'a')
# gps_route_data = open('/kaiyo/log/gsp_route_data/gps_route_data.txt', 'w')
up_down_val = 0
while True:
# padの状態を取得
pad_data = get_pad_data()
go_back_each(pad_data["joy_ly"], pad_data["joy_ry"])
if pad_data["hat_y"]:
time.sleep(0.1)
if up_down_val < 100 and pad_data["hat_y"] == 1:
up_down_val += 10
if up_down_val > -100 and pad_data["hat_y"] == -1:
up_down_val -= 10
up_down(up_down_val)
# gps 取得
if pad_data["btn_a"] == 1:
led_yellow()
gps_data = get_gps_data()
print gps_data
gps_route_data.writelines(str(gps_data) + "\n")
led_off()
if pad_data["btn_x"] == 1:
led_yellow()
gps_route_data.close()
print "pad_rc_route_data_creation END!!"
test()
return 0
def go_yaw_rot(set_speed, set_angle, set_rot, set_diving=True):
set_rot_old = get_data("rot0")
while True:
if set_diving:
diving(set_diving)
#up_down(60)
gol_val = set_angle
# yawを取得して変換( -100 ~ 0 ~ 100)
now_val = my_map(get_data("yaw"))
print "gol_val", gol_val
print "now_val", now_val
# 偏差を調べる
dev_val = gol_val - now_val
if dev_val <= 100:
print "dev_val",dev_val
else:
# 左側を向いていれば、この計算
dev_val = -1*((100 - (-1*now_val)) + (100 - gol_val))
print "dev_val2", dev_val
r = set_speed
l = set_speed
if dev_val >= 0:
dev_val = map15(dev_val, set_speed)
# 右に動く（右を弱める）
r = set_speed - dev_val
r = map13(r, set_speed)
else:
dev_val = map15(dev_val, set_speed)
# 左に動く（左を弱める）
l = set_speed + dev_val
l = map13(l, set_speed)
# if l <= -100:
# l = (200 + l)
print l, r
go_back_each(l, r)
now_rot0 = get_data("rot0")
print "rot---------------------------------",now_rot0 - set_rot_old
print
led_blue()
# 判定範囲
if dev_val >= -1 and dev_val <= 1:
led_lihtblue()
if now_rot0 - set_rot_old >= set_rot:
print "rot stop!!"
stop()
break
def go_yaw_onoff(set_speed, set_angle, set_rot, set_diving=True):
set_rot_old = get_data("rot0")
while True:
if set_diving:
diving(set_diving)
gol_val = set_angle
# yawを取得して変換( -100 ~ 0 ~ 100)
now_val = my_map(get_data("yaw"))
print "gol_val", gol_val
print "now_val", now_val
# 偏差を調べる
dev_val = gol_val - now_val
if dev_val <= 100:
print "dev_val",dev_val
else:
# 左側を向いていれば、この計算
dev_val = -1*((100 - (-1*now_val)) + (100 - gol_val))
print "dev_val2", dev_val
# この範囲の時は直進
if dev_val >= -1 and dev_val <= 1:
r = set_speed
l = set_speed
else:
if dev_val <= 0:
# 左に動かす
r = set_speed
l = set_speed / 2
else:
# 右に動かす
r = set_speed / 2
l = set_speed
print l, r
print
go_back_each(l, r)
now_rot0 = get_data("rot0")
print "rot---------------------------------",now_rot0 - set_rot_old
# print "rot0",now_rot0
led_blue()
# 判定範囲
if dev_val >= -1 and dev_val <= 1:
led_lihtblue()
if now_rot0 - set_rot_old >= set_rot:
print "rot stop!!"
stop()
break
def go_yaw_onoff(speed, angle, set_rot, set_diving=True):
set_rot_old = get_data("rot0")
while True:
if set_diving:
diving(set_diving)
#up_down(60)
gol_val = angle
# yawを取得して変換( -100 ~ 0 ~ 100)
now_val = my_map(get_data("yaw"))
print "gol_val", gol_val
print "now_val", now_val
# 偏差を調べる
dev_val = gol_val - now_val
if dev_val <= 100:
print "dev_val", dev_val
else:
# 左側を向いていれば、この計算
dev_val = -1 * ((100 - (-1 * now_val)) + (100 - gol_val))
print "dev_val2", dev_val
# この範囲の時は直進
if dev_val >= -1 and dev_val <= 1:
# print "Move LR"
r = speed
l = speed
else:
if dev_val <= 0:
# 左に動かす
# print "Move L"
r = speed
# l = 0
l = speed / 2
else:
# 右に動かす
# print "Move R"
# r = 0
r = speed / 2
l = speed
print l, r
print
go_back_each(l, r, 0)
now_rot0 = get_data("rot0")
print "rot000000000000000000000000000000", now_rot0 - set_rot_old
# print "rot0",now_rot0
if now_rot0 - set_rot_old >= set_rot:
print "rot stop!!"
break
def go_yaw_time(set_speed, set_angle, set_time, set_diving=True):
old_time = time.time()
while True:
if set_diving:
diving(set_diving)
#up_down(60)
gol_val = set_angle
# yawを取得して変換
now_val = my_map(get_data("yaw"))
print "gol_val", gol_val
print "now_val", now_val
# 偏差を調べる
dev_val = gol_val - now_val
if dev_val <= 100:
print "dev_val",dev_val
else:
# 左側を向いていれば、この計算
dev_val = -1*((100 - (-1*now_val)) + (100 - gol_val))
print "dev_val2", dev_val
r = set_speed
l = set_speed
if dev_val >= 0:
# 右に動く（右を弱める）
r = set_speed - dev_val
else:
# 左に動く（左を弱める）
l = set_speed + dev_val
go_back_each(l, r)
print l, r
print
ela_time = time.time() - old_time
print ela_time
if ela_time >= set_time:
stop()
break
def pad_rc():
up_down_val = 0
while True:
pad_data = get_pad_data()
# print pad_data
go_back_each(pad_data["joy_ly"], pad_data["joy_ry"])
# if pad_data["joy_rt"]:
# up_down(pad_data["joy_rt"])
# else:
# up_down(-pad_data["joy_lt"])
if pad_data["hat_y"]:
time.sleep(0.1)
if up_down_val < 100 and pad_data["hat_y"] == 1:
up_down_val += 10
if up_down_val > -100 and pad_data["hat_y"] == -1:
up_down_val -= 10
# print up_down_val
up_down(up_down_val)
def go_compass_onoff(set_speed, set_angle, set_rot, set_diving=True):
old_time = time.time()
set_rot_old = get_data("average_rot0_rot1")
while True:
if set_diving:
diving(set_diving)
gol_val = map_compass(set_angle)
# yawを取得して変換( -100 ~ 0 ~ 100)
now_val = my_map(get_data("compass"))
# print "gol_val", gol_val
# print "now_val", now_val
# 偏差を計算
dev_val = gol_val - now_val
if dev_val <= 100:
pass
# print "dev_val",dev_val
else:
# 左側を向いていれば、この計算
dev_val = -1*((100 - (-1*now_val)) + (100 - gol_val))
# print "dev_val2", dev_val
led_blue()
# この範囲の時は直進
if dev_val >= -1 and dev_val <= 1:
led_lihtblue()
r = set_speed
l = set_speed
else:
if dev_val <= 0:
# 左に動かす
r = set_speed
l = set_speed / 2
# l = 0
else:
# 右に動かす
# r = 0
r = set_speed / 2
l = set_speed
# print l, r
# print
go_back_each(l, r)
# now_rot0 = get_data("rot0")
# now_rot0 = get_data("average_rot0_rot1")
now_average_rot0_rot1 = get_data("average_rot0_rot1")
print "now_rot :", now_average_rot0_rot1 - set_rot_old
# ela_time = time.time() - old_time
# if ela_time >= 10:
# # stop()
# break
if now_average_rot0_rot1 - set_rot_old >= set_rot:
# print "\nrot OK!!"
stop()
break
def operation_check():
while True:
my_time = 0.002
print "Do you check the operation? [Y/n]",
key_in = raw_input()
if key_in == "y" or key_in == "Y":
print "Yes operation check!!"
for i in range(100):
go_back_each(i, 0)
print i
time.sleep(my_time)
for i in range(100):
go_back_each(0, i)
print i
time.sleep(my_time)
stop()
for i in range(100):
up_down_each(0, i)
print i
time.sleep(my_time)
for i in range(100):
up_down_each(i, 0)
print i
time.sleep(my_time)
stop()
for i in range(0, -100, -1):
up_down_each(i, 0)
print i
time.sleep(my_time)
for i in range(0, -100, -1):
up_down_each(0, i)
print i
time.sleep(my_time)
stop()
for i in range(0, -100, -1):
go_back_each(0, i)
print i
time.sleep(my_time)
for i in range(0, -100, -1):
go_back_each(i, 0)
print i
time.sleep(my_time)
stop()
for i in range(0, -100, -1):
go_back_each(i, i)
up_down_each(i, i)
print i
time.sleep(my_time)
stop()
for i in range(0, 100, 1):
go_back_each(i, i)
up_down_each(i, i)
print i
time.sleep(my_time)
stop()
print "It worked normally!!"
return 0
elif key_in == "n" or key_in == "N":
print "No operation check!!"
return 0
m_val = 15
while True:
try:
# t1 = time.time()
# go_yaw確認用-------------------------------
print(data["time"], data["yaw"], "yaw_MV :", yaw_MV.value,
"\n")
# その場で補正
# spinturn(yaw_MV.value)
# 前進しながら補正
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)
# go_yaw確認用-------------------------------
# t2 = time.time()
# print("time : ",t2 - t1)
# print("何もない")
time.sleep(0.2)
pass
except KeyboardInterrupt as e:
stop()
break
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
