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 calc():
global maxX
global minX
global maxY
global minY
t_end2 = time.time() + 30
setspeed = 100
#motor = Motor.Motor(18, 25, 24,13, 27, 17)
#time.sleep(5)
while time.time() < t_end2:
magnet = mpu92_forTest.get_magnet()
print(magnet)
if (maxX < magnet[0]):
maxX = magnet[0]
if (minX > magnet[0]):
minX = magnet[0]
if (maxY < magnet[1]):
maxY = magnet[1]
if (minY > magnet[1]):
minY = magnet[1]
# print("maxx",maxX)
# print("minx",minX)
# print()
# print("maxy",maxY)
# print("miny",minY)
# print()
motor.set_speed(-setspeed, setspeed)
time.sleep(1)
def calrad(target_x: float, target_y: float, fix_x: float, fix_y: float):
myGps = get_gps()
if myGps == None:
return 0.0
radT = math.atan2(target_x - myGps[0], target_y - myGps[1])
magnet = mpu92_forTest.get_magnet()
vr = np.array([magnet[0] - fix_x, magnet[1] - fix_y])
rotated_vr = rotation(vr, -radT)
rotated_rad = math.atan2(rotated_vr[1], rotated_vr[0])
return rotated_rad
def calrad(target_x :float, target_y :float):
myGps=get_gps()
if myGps == None:
return 0
radT = math.atan2(target_x - myGps[0],target_y - myGps[1])
magnet = mpu92_forTest.get_magnet()
print(magnet[0]-25)
print(magnet[1]-8)
# x = magnet[0]
# y = magnet[1]
vr = np.array([magnet[0] -25,magnet[1] -8])
rotated_vr = rotation(vr, -radT)
rotated_rad = math.atan2(rotated_vr[1], rotated_vr[0])
return rotated_rad