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_depth(goal):
M = 1.00
M1 = 0.00
e = 0.00
e1 = 0.00
e2 = 0.00
Kp = 0.10
Ki = 0.10
Kd = 0.10
now_depth = get_data("depth")
while (now_depth - 0.4 < now_depth < now_depth + 0.4):
M1 = M
e1 = e
e2 = e1
e = goal - now_depth
M = M1 + Kp * (e - e1) + Ki * e + Kd * ((e - e1) - (e1 - e2))
if goal < now_depth:
direction = 1
else:
direction = -1
if M > 30:
M = 30
elif M < 10:
m = 10
up_down(M * direction)
time.sleep(0.2)
stop()
def t10j_time(set_time=10):
# ジョイスティックの位置を正す
t10j_start_check()
# カウントダウン
for cnt in range(3, 0, -1):
# led_red()
print cnt
time.sleep(0.5)
# led_off()
time.sleep(0.5)
start_time = time.time()
while True:
t10j_map = t10j_map_data()
up_down(t10j_map["ry"])
if t10j_map["lx"] == 0:
go_back(t10j_map["ly"])
else:
spinturn(t10j_map["lx"])
ela_time = time.time() - start_time
print ela_time
if ela_time >= set_time:
stop()
break
def course_pool():
stop()
# Uターン地点まで行く(海上)
led_red()
go_yaw_onoff_iki(set_speed=30, set_rot=400, set_diving=80)
# go_yaw_onoff_iki(set_speed=30, set_rot=200, set_diving=5)
led_off()
# Uターン地点まで行く(潜水)
# led_blue()
# go_yaw_onoff_iki(set_speed=30, set_rot=200, set_diving=60)
# led_off()
# 浮上
stop()
led_green()
up_down(60)
time.sleep(5)
stop()
led_off()
# 設定地点まで自動で移動
waypoint_data = {1:{'lat': 26.377735, 'lng': 127.822441667}}
waypoint(waypoint_data = waypoint_data)
print("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa")
# # Uターン
# # yaw(100, set_diving=False)
# led_purple()
# yaw_rot(set_rot=30, set_diving=True)
# led_off()
# 所定の深さまで沈む
diving_while(30)
# スタート地点まで行く(潜水)
led_blue()
go_yaw_onoff_kaeri(set_speed=30, set_rot=420, set_diving=80)
# go_yaw_onoff_kaeri(set_speed=30, set_rot=210, set_diving=80)
led_off()
# スタート地点まで行く(海上)
# led_red()
# go_yaw_onoff_kaeri(set_speed=30, set_rot=200, set_diving=5)
# led_off()
# 浮上
stop()
led_green()
up_down(60)
time.sleep(2)
my_exit()
def setLedEntry():
global var
var = request.json
print
# print "bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb",sorted(var.items())
print "bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb", var
print
if int(var["slider1"]) == 0:
go_back(int(var["slider2"]))
else:
spinturn(int(var["slider1"]))
up_down(int(var["slider3"]))
def course_convention():
stop()
# Uターン地点まで行く(海上)
led_red()
go_yaw_onoff_iki(set_speed=30, set_rot=250, set_diving=5)
led_off()
# Uターン地点まで行く(潜水)
led_blue()
go_yaw_onoff_iki(set_speed=50, set_rot=880, set_diving=60)
led_off()
# 浮上
stop()
led_green()
up_down(60)
time.sleep(5)
stop()
led_off()
# Uターン
# yaw(100, set_diving=False)
led_purple()
yaw_rot(set_rot=20, set_diving=True)
led_off()
# 所定の深さまで沈む
diving_while(30)
# スタート地点まで行く(潜水)
led_blue()
go_yaw_onoff_kaeri(set_speed=30, set_rot=800, set_diving=60)
led_off()
# スタート地点まで行く(海上)
led_red()
go_yaw_onoff_kaeri(set_speed=30, set_rot=400, set_diving=5)
led_off()
# 浮上
stop()
led_green()
up_down(60)
time.sleep(2)
my_exit()
def diving(val):
# 圧力センサの値を取得
depth = get_data("depth")
# (圧力センサの値) → (0 ~ 100)
map_depth_val = map_depth(depth)
# print "depth", depth
now_val = map_depth_val
gol_val = val
dev_val = gol_val - now_val
# print "now_val", now_val
# print "gol_val", gol_val
# print "dev_val", dev_val
dev_val = map_depth2(dev_val)
# print "motor_out",dev_val
up_down(-dev_val)
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 t10j_mode_sumo():
t10j_start_check()
while True:
t10j_map = t10j_map_data()
print t10j_map
ly = t10j_map["ly"]
lx = t10j_map["lx"]
ry = t10j_map["ry"]
rx = t10j_map["rx"]
# モータ出力
if ly >= 30:
ly = 30
if ry >= 30:
ry = 30
dc_xr(-ry)
dc_xl(ly)
if lx >= 1:
up_down(50)
elif lx <= -1:
up_down(-50)
if lx == 0:
up_down(0)
def diving_while(val):
while True:
depth = get_data("depth")
# 変換
map_depth_val = map_depth(depth)
print "depth", depth
now_val = map_depth_val
gol_val = val
dev_val = gol_val - now_val
print "now_val", now_val
print "gol_val", gol_val
print "dev_val", dev_val
dev_val = map_depth2(dev_val)
print "dev_val_map",dev_val
print
up_down(-dev_val)
if dev_val <= 2 and dev_val >= -2:
print "depth OK !!!"
stop_up_down()
return 0
def my_main(val):
if val == "a":
# jtalk(file_name="modea", voice="エイチワイの皆さんこんにちは、これから、海洋ロボットの説明を始めます。 私のなまえは「ちぶるまぎーもでるじーです」")
jtalk(file_name="modea", voice="皆さんこんにちは、これから、海洋ロボットの説明を始めます。 私のなまえは「ちぶるまぎーもでるじーです」")
if val == "b":
jtalk(file_name="modedb", voice="私は「海洋ロボットコンペティション、イン、沖縄」に出場するためにかいはつされた、海洋ロボットです。")
if val == "c":
jtalk(file_name="modedc", voice="私は主に、アルミニウムとアクリルで構成されています。モータには12ボルトのDCモータ、メインコントローラにはラズベリーパイモデルBを使用しています。バッテリは11.1ボルトのしゅ制御用と「制御回路用」の7.4ボルトのリポバッテリを使用しています。センサ類は主に方向検出用の9軸センサ、水深の検出には圧力センサ、また、各スラスターにはモータの回転数を検出するための「ロータリエンコーダ」を搭載しています。")
# jtalk(file_name="modedc2", voice="潜航深度は最大で5メートル、航行速度は最大で2メートル毎秒です。")
# jtalk(file_name="modedc3", voice="それでは、実際にスラスタを動かしてみたいと思います。機体から少しだけ離れてください。 このスラスタで潜水や浮上を行います。 ")
# jtalk(file_name="modedc4", voice="また、こちらのスラスタでは、前進や行進、旋回などを行います。 ")
# jtalk(file_name="modedc5", voice="また、こちらのLEDは外部からロボットがどのような「動作」をしているかがわかりやすいように7しょくで「現在」の状態を表現しています")
# jtalk(file_name="modedc6", voice="これで海洋ロボットの説明を終了します。ご清聴、ありがとうございました。")
if val == "d":
jtalk_say(file_name="modea")
time.sleep(9)
jtalk_say(file_name="modedb")
time.sleep(10)
# jtalk_say(file_name="modedc")
jtalk_say(file_name="sensor_add")
time.sleep(23)
jtalk_say(file_name="sensor_add2")
time.sleep(40)
jtalk_say(file_name="modedc2")
time.sleep(8)
jtalk_say(file_name="modedc3")
led_blue()
time.sleep(7)
up_down(20)
time.sleep(2)
up_down(90)
time.sleep(3)
stop()
led_off()
jtalk_say(file_name="modedc4")
led_purple()
time.sleep(3)
go_back(20)
time.sleep(2)
go_back(90)
time.sleep(3)
stop()
led_off()
jtalk_say(file_name="modedc5")
led_red()
time.sleep(1.5)
led_blue()
time.sleep(1.5)
led_green()
time.sleep(1.5)
led_purple()
time.sleep(1.5)
led_yellow()
time.sleep(1.5)
led_lihtblue()
time.sleep(1.5)
led_off()
time.sleep(3)
jtalk_say(file_name="modedc6")
while True:
# print("main : " , pad_data)
# print('joy_ly : {}, joy_lx: {}, joy_ry : {}'.format(pad_data["joy_ly"],pad_data["joy_lx"],pad_data["joy_ry"]))
# val_map(joy_ly)
# val_map(joy_lx)
# val_map(joy_ry)
if (pad_data["joy_ly"] == 0 and pad_data["joy_lx"] == 0):
stop()
elif (pad_data["joy_ly"] != 0 and pad_data["joy_lx"] == 0):
go_back(val_map(pad_data["joy_ly"]))
# print("go", val_map(pad_data["joy_ly"]))
elif (pad_data["joy_ly"] == 0 and pad_data["joy_lx"] != 0):
spinturn(val_map(pad_data["joy_lx"]))
# print("spin", val_map(pad_data["joy_lx"]))
if (pad_data["joy_ry"] != 0):
up_down(val_map2(-pad_data["joy_ry"]))
# print("up", val_map(pad_data["joy_ry"]))
# print()
time.sleep(0.1)
except KeyboardInterrupt as key:
# プログラムを終了するときの処理
print("\n-----------")
print("main_demo.py : ", key)
print("-----------\n")
is_stop = 1
def move_test(data, set_input_kb=True):
m_val = 30
path1 = '/kaiyo/log/input_log/20191003/input_log_20191003_122640.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')
# ティーチング用のログを作る-----------------------------------------
# ティーチング用のログを読込む---------------------------------------
elif set_input_kb == False:
try:
with open(path1) 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
# ティーチング用のログを読込む---------------------------------------
line_num = 0
start_time = time.time()
kb = 'p'
print("move_start")
while True:
try:
# print ("input please:")
# if(readchar.readchar() != null):
# ソケット通信からキー入力が来る予定
if set_input_kb == True:
# とりあえずソケットなしでキー入力
kb = readchar.readchar()
# ログのから１行ずつ処理
elif set_input_kb == False:
# print(line_num)
now_line = ast.literal_eval(input_log_file_read[line_num])
# リストがあるなら
if line_num + 1 < fr_len:
next_line = ast.literal_eval(input_log_file_read[line_num +
1])
if line_num == 0:
print("now:", now_line)
print("next", next_line)
print()
# time.sleep(next_line['time'])
kb = str(now_line['inputkey'])
line_num = line_num + 1
else:
print("now:", now_line)
print("next", next_line)
print()
time.sleep(next_line['time'] - now_line['time'])
kb = str(now_line['inputkey'])
line_num = line_num + 1
# print(type(kb))
# print(type(kb))
# print(kb)
# kb = kb.decode()
# print(kb)
# キー入力のログを残す
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(round(end_time -
start_time, 2)) +
", 'inputkey':'" + str(kb) +
"', 'moterval':" + str(m_val) +
"}\n")
# モータ出力------------------------
if kb == 'k': #モータ出力の低下
if m_val > 0:
m_val = m_val - 5
print(m_val)
if kb == 'l': #モータ出力の上昇
if m_val < 65:
m_val = m_val + 5
print(m_val)
# モータ出力------------------------
# モータ一個ずつ動かす------------------------
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 == 'x': #後退
go_back(-m_val)
print("後退:", m_val)
if kb == 'q': #潜水
up_down(-m_val)
print("潜水:", m_val)
# if kb == 'f': #潜水
# up_down(depth_MV)
# 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 == 's': #全停止
stop()
print("全停止:")
if kb == 'r': #推進用停止
stop_go_back()
print("推進用停止:")
if kb == 't': #潜水用停止
stop_up_down()
print("潜水用停止:")
# 停止---------------------------------------
if kb == 'b': #指定時間潜水
up_down(m_val)
print("潜水:", m_val)
time.sleep(10)
stop()
if kb == 'p': #終了
# my_exit()
stop()
print("終了します")
break
# ティーチングのとき使うかもしれないからリセット
kb = None
# print(kb)
except KeyboardInterrupt as e:
stop()
# prtin("move:Keyerror")
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
