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() # ログのから1行ずつ処理 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: # ログのから1行ずつ処理 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軸センサ、潜っている深さの検出には圧力センサ、波の強さを検出するための流量計、また、各スラスターにはモータの回転数を検出するための「ロータリエンコーダー」、モータにかかる負荷を監視するための「電流計」、モータの温度を確認するための「温度センサ」を搭載しています。また、前方と下方向の様子を確認するための「カメラ」を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 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()