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