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 waypoint(waypoint_data):
for route_number, lat_lng_data in waypoint_data.items():
print "-" * 60
print "route_number :", route_number
print "lat :", lat_lng_data["lat"]
print "lng :", lat_lng_data["lng"]
goal_lat = lat_lng_data["lat"]
goal_lng = lat_lng_data["lng"]
while True:
# up_down(40)
# 目標地点までの角度と距離を取得
goal_gps_data = get_direction_distance(goal_lat=goal_lat,
goal_lng=goal_lng)
# 目的地と現在地が直径1m以下なら次の目的地
if goal_gps_data["distance"] <= 1:
for i in range(10):
led_purple()
time.sleep(0.1)
led_off()
time.sleep(0.1)
print "Next waypoint!!"
break
# 移動開始
else:
print "Try waypoint!!"
# その場で目標地点の角度(direction)を向く
compass(set_angle=goal_gps_data["direction"], set_diving=False)
# 目標地点まで角度を調整しながら前進
# go_compass_onoff(set_speed=30, set_angle=0, set_rot=goal_gps_data["set_rot"], set_diving=True)
go_compass_onoff(set_speed=30,
set_angle=goal_gps_data["direction"],
set_rot=30,
set_diving=True)
def battery_check(set_lipoC2=8, set_lipoC3S3=11):
data = get_data("all")
print
print "----------------------------------"
print "Lipo checking!!"
time.sleep(0.5)
if data["lipoC2"] <= set_lipoC2 or data["lipoC3S3"] <= set_lipoC3S3:
if data["lipoC2"] <= 1 or data["lipoC3S3"] <= 1:
if data["lipoC2"] <= 1:
print "lipoC2 : No connection!!"
if data["lipoC3S3"] <= 1:
print "lipoC3S3 : No connection!!"
else:
while True:
if data["lipoC2"] <= set_lipoC2:
print "lipoC2 : "+str(get_data("lipoC2"))+"[V]"
if data["lipoC3S3"] <= set_lipoC3S3:
print "lipoC3S3 : "+str(get_data("lipoC3S3"))+"[V]"
print "Lipo Low!!"
led_red()
time.sleep(0.1)
led_off()
time.sleep(0.1)
print "lipoC2 : "+str(get_data("lipoC2"))+"[V]"
print "lipoC3S3 : "+str(get_data("lipoC3S3"))+"[V]"
print "Status okay!!"
print "----------------------------------"
print
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 atumeru_led():
time_led = 0.1
for i in range(12):
led_red()
time.sleep(time_led)
led_red()
time.sleep(time_led)
led_green()
time.sleep(time_led)
led_purple()
time.sleep(time_led)
led_yellow()
time.sleep(time_led)
led_lihtblue()
time.sleep(time_led)
led_off()
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 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")
# 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 my_exit():
led_off()
stop()
sys.exit()
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 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 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 !!")
