def diving(val): print "depth!!" # 圧力センサの値を取得 depth = get_data("depth") # (圧力センサの値) → (0 ~ 100) map_depth_val = map_depth(depth) print "depth", depth now_val = map_depth_val gol_val = val dev_val = gol_val - now_val print "now_val", now_val print "gol_val", gol_val print "dev_val", dev_val dev_val = map_depth2(dev_val) print "motor_out", dev_val print up_down(-dev_val)
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 t10j_map_data(): data = get_data("all") # print "LY LX RY RX" t10j_data = [data["duty2"], data["duty0"], data["duty1"], data["duty3"]] # print t10j_data t10j_map = [] for val in t10j_data: in_min = 1104 in_max = 1918 out_min = -100 out_max = 100 val = (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min if val < -100: val = -100 if val > 100: val = 100 if val <= 10 and val >= -10: val = 0 t10j_map.append(val) t10j_map = {"ly":t10j_map[0],"lx":t10j_map[1],"ry":t10j_map[2],"rx":t10j_map[3]} # print t10j_map return t10j_map
def diving_while(val): while True: depth = get_data("depth") # 変換 map_depth_val = map_depth(depth) print "depth", depth now_val = map_depth_val gol_val = val dev_val = gol_val - now_val print "now_val", now_val print "gol_val", gol_val print "dev_val", dev_val dev_val = map_depth2(dev_val) print "dev_val_map",dev_val print up_down(-dev_val) if dev_val <= 2 and dev_val >= -2: print "depth OK !!!" stop_up_down() return 0
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 sensor_gps_log(set_sample_rate=0.2): try: os.makedirs('/2019_auv/log/sensor_log/' + str(datetime.now().strftime('%Y%m%d'))) except FileExistsError: pass try: os.makedirs('/2019_auv/log/gps_log/' + str(datetime.now().strftime('%Y%m%d'))) except FileExistsError: pass try: os.makedirs('/2019_auv/log/join_log/' + str(datetime.now().strftime('%Y%m%d'))) except FileExistsError: pass if set_sensor_log: sensor_log_file_time = open( '/2019_auv/log/sensor_log/' + str(datetime.now().strftime('%Y%m%d')) + '/sensor_log_' + str(datetime.now().strftime('%Y%m%d_%H%M%S')) + '.txt', 'a') if set_gps_log: gps_log_file_time = open( '/2019_auv/log/gps_log/' + str(datetime.now().strftime('%Y%m%d')) + '/gps_log_' + str(datetime.now().strftime('%Y%m%d_%H%M%S')) + '.txt', 'a') if set_sensor_gps_log: gps_sensor_log_file_time = open( '/2019_auv/log/join_log/' + str(datetime.now().strftime('%Y%m%d')) + '/gps_sensor_log_' + str(datetime.now().strftime('%Y%m%d_%H%M%S')) + '.txt', 'a') start_time = time.time() session = gps.gps("localhost", "2947") session.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE) lat = "" lon = "" alt = "" while True: # Arduino から一行取得 data = get_data("all") try: # サンプリングレート以上時間が経過したら書き込み ela_time = time.time() - start_time if ela_time >= set_sample_rate: i = 0 #sensor-------------------------------------- data["datetime"] = str(datetime.now()) if set_sensor_log: sensor_log_file_time.writelines(str(data) + "\n") #sensor-------------------------------------- #gps----------------------------------------- report = next(session) # print report # To see all report data, uncomment the line below if report['class'] == 'TPV': if hasattr(report, 'lat'): lat = float(report.lat) if hasattr(report, 'lon'): lon = float(report.lon) if hasattr(report, 'alt'): alt = float(report.alt) if i >= 10: i = 0 if (lat != "" and lon != "" and alt != ""): gps_data_dict = { "lat": lat, "lng": lon, "alt": alt } # str に変換 gps_data_dict["datetime"] = str(datetime.now()) # log に書き込み if set_gps_log: gps_log_file_time.writelines( str(gps_data_dict) + "\n") else: i += 1 #gps----------------------------------------- #join---------------------------------------- data["lat"] = lat data["lon"] = lon data["alt"] = alt if set_sensor_gps_log: gps_sensor_log_file_time.writelines(str(data) + "\n") #join---------------------------------------- start_time = time.time() except SyntaxError: # 受信エラー print("Reception Error!!") except KeyError: pass except KeyboardInterrupt: quit() except StopIteration: session = None print("GPSD has terminated!!")
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
print t10j_map # up_down(t10j_map[3]) if t10j_map[1] == 0: # pass go_back(t10j_map[0]) else: # pass spinturn(t10j_map[1]) return 0 # ------------------------------------------------------------------- if __name__ == '__main__': try: ini_val = get_data("yaw") # センサー初期化 send_data("reboot") while True: data = get_data("all") # プロポで操作 t10j(data) stop() except KeyboardInterrupt as e: stop() #Ctrl + Cを押したときの処理
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 voice_check(val): data = get_data("all") print data[val] time.sleep(1)
def motor_tuning(): global xr_cor, xl_cor, yr_cor, yl_cor # 各モータに対応する補正値 flag = [True] * 7 # 一回だけ実行するためのフラグ fix_flag = True # 出力の強いモータを調べるために使うフラグ rool = 100 # 回転数 val = 30 # dety比(モータ出力) eta = 0.02 # 学習率 goal = 0.02 # 目標誤差 t0 = time.perf_counter() # スタート時間 while (True): if flag[0] == True: # 値のリセットが完了するまで待機-------------------------------- while True: stop() send_data("reboot") time.sleep(1) data = get_data("all") rot0 = data["rot0"] rot1 = data["rot1"] rot2 = data["rot2"] rot3 = data["rot3"] print(t0, rot0, rot1, rot2, rot3) if (rot0 == 0 and rot1 == 0 and rot2 == 0 and rot3 == 0): print("start") flag[0] = False start_time = time.perf_counter() # スタート時間 xl(val) xr(-val) yl(val) yr(-val) break else: print("reboot") # 値のリセットが完了するまで待機------------------------------- data = get_data("all") rot0 = data["rot0"] rot1 = data["rot1"] rot2 = data["rot2"] rot3 = data["rot3"] # 各モータごとに指定の回転回数を超えたか判定----------------- if rot0 >= rool: if flag[1] == True: xr(0) xr_time = time.perf_counter() print("rot0_time : ", xr_time - start_time) flag[1] = False else: pass if rot1 >= rool: if flag[2] == True: xl(0) xl_time = time.perf_counter() print("rot1_time : ", xl_time - start_time) flag[2] = False else: pass if rot2 >= rool: if flag[3] == True: yl(0) yl_time = time.perf_counter() print("rot2_time : ", yl_time - start_time) flag[3] = False else: pass if rot3 >= rool: if flag[4] == True: yr(0) yr_time = time.perf_counter() print("rot3_time : ", yr_time - start_time) flag[4] = False else: pass # 各モータごとに指定の回転回数を超えたか判定----------------- # すべてのモータ回転終了 if (rot0 >= rool and rot1 >= rool and rot2 >= rool and rot3 >= rool): print(rot0, rot1, rot2, rot3) print("time_dif go: ", XR_time - t3) print("time_dif up: ", yl_time - yr_time) # モータをひとつ固定してもう一方を調整(今のままだと補正値1.0超える)---- if (XR_time - xl_time) >= goal: # 0.02以上 xr_cor += eta elif (XR_time - xl_time) <= -goal: # -0.02以下 xr_cor -= eta else: print("go_back_OK") flag[5] = False if (yl_time - yr_time) >= goal: yl_cor += eta elif (yl_time - yr_time) <= -goal: yl_cor -= eta else: print("up_down_OK") flag[6] = False # モータをひとつ固定してもう一方を調整(今のままだと補正値1.0超える)---- print(xr_cor, xl_cor, yl_cor, yr_cor) # モータ終了時間の差が指定時間(0.02)以内 if (flag[5] == False and flag[6] == False): print("end") break else: #もう一度 flag = [True] * 7 t6 = time.perf_counter() print("all_time : ", t6 - t0) ini_set('set_mode', 'rot0_cor', str(xr_cor)) ini_set('set_mode', 'rot1_cor', str(xl_cor)) ini_set('set_mode', 'rot2_cor', str(yl_cor)) ini_set('set_mode', 'rot3_cor', str(yr_cor)) print(xr_cor, xl_cor, yl_cor, yr_cor) return True
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 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()
def my_main(): # センサーデータ取得 data = get_data("all")