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 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 t10j_start_check():
while True:
t10j_map = t10j_map_data()
print "RY joystick upper!!"
# if 100 == t10j_map["ly"] and t10j_map["lx"] and t10j_map["ry"] and t10j_map["rx"]:
if 100 == t10j_map["ry"]:
led_green()
while True:
t10j_map = t10j_map_data()
print "RY joystick center!!"
if 0 == t10j_map["ry"]:
while True:
print "Joysticks OK!!"
led_blue()
return 0
def compass(set_angle, set_diving=True):
while True:
if set_diving:
diving(set_diving)
# up_down(40)
# 180 ~ 0 ~ -180 -> 100 ~ 0 -100
# print map_compass(set_angle)
# set_angle = map_compass(set_angle)
gol_val = map_compass(set_angle)
# print gol_val
# (0 ~ 100) → (-100 ~ 0 ~ 100)
now_val = my_map(get_data("compass"))
# (-100 ~ 0 ~ 100) → (0 ~ 200)
now_val2 = map_yaw2(now_val)
# print "gol_val", gol_val
# print "now_val", now_val
# 偏差を調べる
dev_val = now_val2 - gol_val
if dev_val >= 101:
dev_val = -(200 - dev_val)
led_blue()
# 目標角度になったら終了
if dev_val <= 2 and dev_val >= -2:
# if dev_val <= 0 and dev_val >= 0:
led_lihtblue()
# print "\ndirection OK!!"
stop_go_back()
return 0
# モータの出力を調整(-100 ~ 0 ~ 100) → (-60 ~ 0 ~ 60)
# メカナム用
dev_val = map_yaw_adjustment(dev_val)
# if dev_val <= -1 and dev_val >= -40:
# dev_val = -35
# if dev_val >= 1 and dev_val <= 40:
# dev_val = 35
spinturn(-dev_val)
sys.stdout.write("\rdev_val : %d" % -dev_val)
sys.stdout.flush()
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 yaw(set_angle, set_diving=True):
while True:
if set_diving:
diving(set_diving)
gol_val = set_angle
# (-180 ~ 0 ~ 180) → (-100 ~ 0 ~ 100)
gol_val = map_compass(-set_angle)
# (0 ~ 100) → (-100 ~ 0 ~ 100)
now_val = my_map(get_data("yaw"))
# now_val = my_map(get_data("compass"))
# (-100 ~ 0 ~ 100) → (0 ~ 200)
now_val2 = map_yaw2(now_val)
print "gol_val", gol_val
print "now_val", now_val
# 偏差を調べる
dev_val = now_val2 - gol_val
if dev_val >= 101:
dev_val = -(200 - dev_val)
led_blue()
# 目標角度になったら終了
if dev_val <= 2 and dev_val >= -2:
# if dev_val <= 0 and dev_val >= 0:
led_lihtblue()
print "balance OK!!"
stop_go_back()
return 0
# モータの出力を調整(-100 ~ 0 ~ 100) → (-60 ~ 0 ~ 60)
dev_val = map_yaw_adjustment(dev_val)
spinturn(-dev_val)
print "dev_val", -dev_val
print "motor_out", -dev_val
print
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_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
