rotation_lock = threading.Lock()
URwC_flag = 1
pi = pigpio.pi()
DMUX_pin = [11, 9, 10] # マルチプレクサの出力指定ピンA,B,C
DMUX_out = [0, 0, 0] # 出力ピン指定のHIGH,LOWデータ
for pin in range(0, 2):
pi.set_mode(DMUX_pin[pin], pigpio.OUTPUT)
pi.write(DMUX_pin[pin], DMUX_out[pin])
pi.set_mode(pinL, pigpio.OUTPUT)
pi.set_mode(pinR, pigpio.OUTPUT)
p = pid_controll.pid(0.004, 0.03, 0.0004)
mpu = MPU6050.MPU6050(0x68)
def update_rotation_with_cam():
global rotation
cap = capture.capture()
cam = camera.CamAnalysis()
while URwC_flag == 1:
stream = cap.cap()
cam.morphology_extract(stream)
cam.save_all_outputs()
coord = cam.contour_find()
conX = ((coord[0] - width / 2) / (width / 2)) / math.sqrt(3)
rotation_lock.acquire()
rotation = math.degrees(math.atan(-conX))
while 1:
pre = GPS.lat_long_measurement()
if pre[0] != None:
break
print(pre)
try:
index = 0
filename = 'gps_hom_log' + '%04d' % index
while os.path.isfile(current_dir + '/' + filename + '.csv') == True:
index += 1
filename = 'gps_hom_log' + '%04d' % index
DMUX_out = [0, 0, 0]
for pin in range(0, 2):
pi.write(DMUX_pin[pin], DMUX_out[pin])
MPU = MPU6050.MPU6050(0x68)
pi.set_mode(pinL, pigpio.OUTPUT)
pi.set_mode(pinR, pigpio.OUTPUT)
to_goal, rotation = [1, 0], 0
t1 = threading.Thread(target=gps_get)
t2 = threading.Thread(target=gyro_get)
t1.start()
t2.start()
with open(current_dir + '/' + filename + '.csv', 'w') as c:
csv_writer = csv.writer(c, lineterminator='\n')
while 1:
pi.hardware_PWM(pinL, 50, int(dL))
pi.hardware_PWM(pinR, 50, int(dR))
if to_goal[0] < cam_dis:
pi.hardware_PWM(pinL, 50, 75000)