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)
