from machine import PWM, Pin
import utime
import soukou as sk
IN1 = PWM(Pin(11,Pin.OUT))
IN2 = PWM(Pin(12,Pin.OUT))
IN3 = PWM(Pin(13,Pin.OUT))
IN4 = PWM(Pin(14,Pin.OUT))
IN1.freq(1000)
IN2.freq(1000)
IN3.freq(1000)
IN4.freq(1000)
IN1.duty_u16(0)
IN2.duty_u16(0)
IN3.duty_u16(0)
IN4.duty_u16(0)
sensor_1 = Pin(27,Pin.IN)
sensor_2 = Pin(26,Pin.IN)
sensor_3 = Pin(28,Pin.IN)
Q1 = int(12500*1.8*2)
Q2 = int(10000*1.8*2)
while True:
s1 = sensor_1.value()
s2 = sensor_2.value()
s3 = sensor_3.value()
utime.sleep(0.001)
print(s1,s2,s3)
utime.sleep(0.002)
if(s1 == 1 and s2 == 1 and s3 == 1): #if(A0 == 0 and A1 == 0 and A2 == 0):
print ("黒線に乗っています。")
sk.forward(Q1,Q1)
if(s1 == 1 and s2 == 0 and s3 == 1): #if(A0 == 0 and A1 == 0 and A2 == 0):
print ("線から右にそれました。")
sk.left(Q1,Q1)
if(s1 == 1 and s2 == 1 and s3 == 0): #if(A0 == 0 and A1 == 0 and A2 == 0):
print ("線から左にそれました。")
sk.right(Q1,Q1)
if(s1 == 0 and s2 == 0 and s3 == 0): #if(A0 == 0 and A1 == 0 and A2 == 0):
print ("線から完全にそれました。")
sk.back(Q1,Q1)