def main():
print("始まったよ")
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.set_xlim(-50, -30)
ax.set_ylim(65, 85)
x = 0
y = 0
print("matlab準備")
mpu92_forTest.MPU9265_init()
magnet = mpu92_forTest.get_magnet()
x = magnet[0]
y = magnet[1]
ax.grid(color='gray')
lines, = ax.plot(x, y, marker="o")
while True:
#点の座標の更新
magnet = mpu92_forTest.get_magnet()
x = magnet[0]
y = magnet[1]
print(x)
print(y)
print()
#新たな点をプロット
ax.plot(x, y, marker="o", c="dodgerblue")
plt.pause(.01)
def main():
#set up
#センサーセットアップ
init()
mpu92_forTest.MPU9265_init()
#目標をセット
target_location_x = 34.805843
target_location_y = 135.778176
target_rad = 0.0
#中心
calc()
center_x = get_cecter_x(maxX, minX)
center_y = get_cecter_y(maxY, minY)
print(center_x)
print(center_y)
#roverの状態（フェーズ）
phase = 0
#move
while True:
if phase == 0: #x秒前進
print("phase:0")
motor.set_speed(100, 100)
time.sleep(5) #単位は秒
phase = 1 #次のフェーズへ
elif phase == 1:
print("phase:1")
if -0.001 < (get_gps()[0] - target_location_x) and (
get_gps()[0] - target_location_x) < 0.001: # 緯度の差が0.01以内で
if -0.001 < (get_gps()[1] - target_location_y) and (
get_gps()[1] -
target_location_y) < 0.001: # かつ経度の差が0.01以内ならば
phase = 2 # 次のフェーズへ
continue
target_rad = calrad(target_location_x, target_location_y, center_x,
center_y)
print(target_rad * (180 / np.pi))
if target_rad > 0.174533: #0.174533=10度
print("左回転") #target_radを減らす方向
motor.set_speed(50, 100)
elif target_rad < -0.174533:
print("右回転") # target_radを増やす方向
motor.set_speed(100, 50)
else: # -10度< Θ < 10度
print("直進")
motor.set_speed(100, 100)
time.sleep(1)
elif phase == 2: #カメラモード
print("phase:2")
phase = 3
elif phase == 3:
print("phase:3")
break
GPIO.cleanup()
print("正常終了")
def main():
init()
mpu92_forTest.MPU9265_init()
target_location_x= 34.805843
target_location_y= 135.778176
target_rad=0
while True:
target_rad = calrad(target_location_x,target_location_y)
print("target_rad")
print(target_rad * (180/np.pi))
print()
if target_rad >0.174533 :#0.174533=10度
motor.set_speed(-100,100)
elif target_rad < -0.174533:
motor.set_speed(100,-100)
else:
pass
time.sleep(1)
def main():
#set up
#センサーセットアップ
init()
mpu92_forTest.MPU9265_init()
#目標をセット
target_location_x = 34.805843
target_location_y = 135.778176
target_rad = 0.0
#中心
calc()
center_x = get_cecter_x(maxX, minX)
center_y = get_cecter_y(maxY, minY)
print(center_x)
print(center_y)
#roverの状態（フェーズ）
phase = 0
#move
while True:
pass
def main():
#set up
#センサーセットアップ
# motor.set_speed(40, -40)
# time.sleep(1.5)
# motor.set_speed(80, -80)
# time.sleep(1.5)
# motor.set_speed(50, -50)
# time.sleep(1.5)
init()
mpu92_forTest.MPU9265_init()
#落下
time.sleep(30)
#ニクロム線カット
nikurom()
time.sleep(15)
#前進
motor.set_speed(-40, 40)
time.sleep(2)
motor.set_speed(-80, 80)
time.sleep(2)
motor.set_speed(-50, 50)
time.sleep(2)
#目標をセット34.800286, 135.769161
target_location_x = 135.769161
target_location_y = 34.800286
target_rad = 0.0
#中心
# calc()
# center_x = get_cecter_x(maxX,minX)
# center_y = get_cecter_y(maxY,minY)
center_x = -39.6240234375
center_y = 75.29296875
print(center_x)
print(center_y)
#roverの状態（フェーズ）
phase = 1
#move
while True:
if phase == 0: #x秒前進
print("phase:0")
motor.set_speed(-100, 100)
time.sleep(1) #単位は秒
phase = 1 #次のフェーズへ
elif phase == 1:
print("phase:1")
print(get_gps()[0], get_gps()[1])
if -0.00005 < (get_gps()[0] - target_location_x) and (
get_gps()[0] -
target_location_x) < 0.00005: # 緯度の差が0.01以内で
if -0.00005 < (get_gps()[1] - target_location_y) and (
get_gps()[1] -
target_location_y) < 0.00005: # かつ経度の差が0.01以内ならば
phase = 2 # 次のフェーズへ
continue
target_rad = calrad(target_location_x, target_location_y, center_x,
center_y)
print("target_rad")
print(target_rad * (180 / np.pi))
if target_rad > 0.174533: #0.174533=10度
print("左回転") #target_radを減らす方向
motor.set_speed(-100, 50)
elif target_rad < -0.174533:
print("右回転") # target_radを増やす方向
motor.set_speed(-50, 100)
else: # -10度< Θ < 10度
print("直進")
motor.set_speed(-100, 100)
time.sleep(1)
elif phase == 2: #カメラモード
print("phase:2")
phase = 3
elif phase == 3:
print("phase:3")
GPIO.cleanup()
break
GPIO.cleanup()
print("正常終了")