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軸センサ、潜っている深さの検出には圧力センサ、波の強さを検出するための流量計、また、各スラスターにはモータの回転数を検出するための「ロータリエンコーダー」、モータにかかる負荷を監視するための「電流計」、モータの温度を確認するための「温度センサ」を搭載しています。また、前方と下方向の様子を確認するための「カメラ」を2つ、さらに私の向いている方向を、操縦者が目視で確認できるように「スマートフォン」を搭載しています。") # 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 !!")