from machine import Pin, I2C
from time import *
from ssd1306 import SSD1306_I2C

sleep(0.1) # Wait for USB to become ready

WIDTH = 128
HEIGHT = 64
i2c = I2C(0,scl=Pin(17),sda=Pin(16),freq=200000)

oled = SSD1306_I2C(WIDTH,HEIGHT,i2c)

btn_red = Pin(21, Pin.IN)
btn_blue = Pin(20, Pin.IN)

led_red = Pin(15, Pin.OUT)
led_blue = Pin(0, Pin.OUT)


Am1 = Pin(13, Pin.OUT)
Ap1 = Pin(12, Pin.OUT)
Bp1 = Pin(10, Pin.OUT)
Bm1 = Pin(11, Pin.OUT)

Am2 = Pin(9, Pin.OUT)
Ap2 = Pin(8, Pin.OUT)
Bp2 = Pin(7, Pin.OUT)
Bm2 = Pin(6, Pin.OUT)

def engine(Ap, Am, Bm, Bp):
    
    sleep_ms(1)
    Bm.value(0)
    Ap.value(1)

    sleep_ms(1)
    Ap.value(0)
    Bp.value(1)

    sleep_ms(1)
    Bp.value(0)
    Am.value(1)

    sleep_ms(1)
    Am.value(0)
    Bm.value(1)

def engine2():
    
    sleep_ms(1)
    Bm2.value(0)
    Ap2.value(1)

    sleep_ms(1)
    Ap2.value(0)
    Bp2.value(1)

    sleep_ms(1)
    Bp2.value(0)
    Am2.value(1)

    sleep_ms(1)
    Am2.value(0)
    Bm2.value(1)


while 1:
    if btn_red.value():
        led_blue.toggle()
        led_red.toggle()
        sleep(0.1)

        if led_blue.value() == 1:
            if led_red.value() == 1:
                while 1:
                    engine(Ap1, Am1, Bm1, Bp1)
                    engine(Ap2, Am2, Bm2, Bp2)
                    oled.text("LED,MOTOR is ON",1,1)
                    oled.show()

                    if btn_blue.value():
                        led_blue.toggle()
                        led_red.toggle()
                        sleep(0.1)
                        
                        if led_blue.value() == 0:
                            if led_red.value() == 0:
                                oled.text("LED,MOTOR is OFF", 1,1)
                                oled.show()
                                
                            break
                            sleep(0.1)