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")
