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 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 go_depth(goal_depth, data, yaw_depth):
while True:
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 = data("depth")
while (now_depth - 0.4 < now_depth < now_depth + 0.4):
M1 = M
e1 = e
e2 = e1
e = goal_depth - now_depth
M = M1 + Kp * (e - e1) + Ki * e + Kd * ((e - e1) - (e1 - e2))
if goal_depth < 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 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()
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
# -------------------------------------------------------------------
if __name__ == '__main__':
try:
ini_val = get_data("yaw")
# センサー初期化
# send_data("reboot")
while True:
t10j(set_time=10)
stop()
except KeyboardInterrupt as e:
stop() #Ctrl + Cを押したときの処理
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 go_yaw(set_speed, set_angle, set_rot=False, set_time=False, set_diving=True):
set_rot_old = get_data("rot0")
old_time = time.time()
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)
led_blue()
# 判定範囲
if dev_val >= -1 and dev_val <= 1:
led_lihtblue()
if set_rot:
# now_rot0 = get_data("rot0")
now_average_rot0_rot1 = get_data("average_rot0_rot1")
print now_average_rot0_rot1
print "rot---------------------------------", now_average_rot0_rot1 - set_rot_old
# print "rot---------------------------------", set_rot_old - now_average_rot0_rot1
print
if now_average_rot0_rot1 - set_rot_old >= set_rot:
print "rot stop!!"
stop()
break
if set_time:
ela_time = time.time() - old_time
print ela_time
if ela_time >= set_time:
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
def my_exit():
led_off()
stop()
sys.exit()
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 my_exit():
led_off()
GPIO.cleanup()
stop()
sys.exit()
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
# jtalk(file_name="sensor_add", voice="")
# jtalk(file_name="sensor_add", voice="私は主に、アルミニウムとアクリルで構成され,、腐食を防ぐために表面はアルマイト処理されています。モータには12ボルトのDCモータ、メインコントローラにはラズベリーパイモデルビーーを使用しています。バッテリは11.1ボルトの主制御用と「制御回路用」の7.4ボルトのリポバッテリを使用しています。")
# jtalk(file_name="sensor_add2", voice="センサ類は主に方向検出用の9軸センサ、潜っている深さの検出には圧力センサ、波の強さを検出するための流量計、また、各スラスターにはモータの回転数を検出するための「ロータリエンコーダー」、モータにかかる負荷を監視するための「電流計」、モータの温度を確認するための「温度センサ」を搭載しています。また、前方と下方向の様子を確認するための「カメラ」を２つ、さらに私の向いている方向を、操縦者が目視で確認できるように「スマートフォン」を搭載しています。")
# my_main(val = "d")
try:
while True:
my_main(val = "d")
time.sleep(60)
# jtalk(file_name="min3", voice="皆さんこんにちは、今から3分後に自動デモンストレーションを開始します。ご覧になるかたは集まってください。")
jtalk_say(file_name="min3")
atumeru_led()
time.sleep(60)
# jtalk(file_name="min2", voice="皆さんこんにちは、今から2分後に自動デモンストレーションを開始します。ご覧になるかたは集まってください。")
jtalk_say(file_name="min2")
atumeru_led()
time.sleep(60)
# jtalk(file_name="min1", voice="皆さんこんにちは、今から1分後に自動デモンストレーションを開始します。ご覧になるかたは集まってください。")
jtalk_say(file_name="min1")
atumeru_led()
time.sleep(50)
# jtalk(file_name="sec10", voice="皆さんこんにちは、今から10秒後に自動デモンストレーションを開始します。ご覧になるかたは集まってください。")
jtalk_say(file_name="sec10")
atumeru_led()
time.sleep(10)
except KeyboardInterrupt as e:
print("\nCtrl-c!!")
led_off()
stop()
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 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")
def go_yaw(goal):
M = 0.00
M1 = 0.00
e = 0.00
e1 = 0.00
e2 = 0.00
Kp = 0.001
Ki = 0.01
Kd = 0.5
data = get_data("all")
now_yaw = data["yaw"]
if now_yaw < 0:
now_yaw = 360 + now_yaw
if goal < 0:
goal = 360 + goal
if goal == 0:
while (not (now_yaw > 359 or now_yaw < 1)):
data = get_data("all")
now_yaw = data["yaw"]
M1 = M
e1 = e
e2 = e1
if now_yaw < 0:
now_yaw = 360 + now_yaw
if abs(goal - now_yaw) > 180:
e = 360 - abs(goal - now_yaw)
else:
e = abs(goal - now_yaw)
M = M1 + Kp * (e - e1) + Ki * e + Kd * ((e - e1) - (e1 - e2))
print("now_yaw : ", now_yaw)
direction = roteto(now_yaw, goal)
if M > 30:
M = 30
elif M < 10:
M = 10
spinturn(M * direction)
else:
while (not (now_yaw - 1 < goal < now_yaw + 1)):
data = get_data("all")
now_yaw = data["yaw"]
M1 = M
e1 = e
e2 = e1
if now_yaw < 0:
now_yaw = 360 + now_yaw
if abs(goal - now_yaw) > 180:
e = 360 - abs(goal - now_yaw)
else:
e = abs(goal - now_yaw)
M = M1 + Kp * (e - e1) + Ki * e + Kd * ((e - e1) - (e1 - e2))
direction = roteto(now_yaw, goal)
if M > 30:
M = 30
elif M < 10:
M = 10
spinturn(M * direction)
stop()
