cam_interval = 1.5 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 = 0 drift = -1.032555 gpio.setmode(gpio.BCM) gpio.setwarnings(False) gpio.setup([11, 9, 10, 13, 18], gpio.OUT) gpio.output(11, False) gpio.output(9, False) gpio.output(10, False) svL, svR = gpio.PWM(13, 50), gpio.PWM(18, 50) svL.start(6.9) svR.start(6.9) mpu = MPU6050.MPU6050(0x68) p = pid_controll.pid(0.004, 0.02365, 0.0002436) #p = pid_controll.pid(4.8, 23.65, 0.2436) pt = time.time() def rotate(self, duty): self.srv.ChangeDutyCycle(duty) try: while True: gyro = mpu.get_gyro_data_lsb()[2] + drift nt = time.time() dt = nt - pt pt = nt rotation += gyro * dt m = p.update_pid(90, rotation, dt)