from machine import Pin
from machine import PWM
from time import sleep

class Motor:
    def __init__(self, dirpin, speedpin):
        self.dirpin = Pin(dirpin, Pin.OUT)
        self.speedpin = PWM(Pin(speedpin))
        print(self.speedpin.freq(50))
        print(self.speedpin.duty())
    
    def off(self):
        self.dirpin.off()
        self.speedpin.duty(0)
        
    def forward(self, speed):
        self.dirpin.off()
        self.speedpin.duty(speed)
        
    def reverse(self, speed):
        self.dirpin.on()
        self.speedpin.duty(1023-speed)
        
    
sensorpins = [15, 2, 4, 16, 17, 5, 18]
sensors = []

for i, pin in enumerate(sensorpins):
    print(pin)
    sensors.append(Pin(pin, Pin.IN))
    print(sensors[i])

left = Motor(1, 3)
right = Motor(22, 23)

left.off()
right.off()

#left.forward(512)
#right.reverse(512)

print("Started")
#left.off()
#right.on()

try:
    # while True:
    #     if sensors[0].value() == 1:
    #         print("Left")
    #         left.off()
    #         right.forward(400)
    #     if sensors[1].value() == 1:
    #         left.forward(300)
    #         right.forward(600)
        
    #     if sensors[2].value() == 1:
    #         left.forward(400)
    #         right.forward(800)
        
    #     if sensors[3].value() == 1:
    #         left.forward(800)
    #         right.forward(800)
    #         print("Forward")
        
    #     if sensors[4].value() == 1:
    #         left.forward(800)
    #         right.forward(400)
        
    #     if sensors[5].value() == 1:
    #         left.forward(600)
    #         right.forward(300)

    #     if sensors[6].value() == 1:
    #         print("Right")
    #         left.forward(400)
    #         right.off()
        
    pass
finally:
    left.off()
    right.off()