from machine import Pin, I2C
from i2c_lcd import I2cLcd
import time

i2c = I2C(0, scl=Pin(17), sda=Pin(16), freq=400000)
addr = i2c.scan()[0]
print("i2c Address : ",addr)
lcd = I2cLcd(i2c, addr, 2, 16)

M1_step = Pin(20, Pin.OUT)
M1_dir  = Pin(21, Pin.OUT)
M2_step = Pin(18, Pin.OUT)
M2_dir  = Pin(19, Pin.OUT)
M3_step = Pin(26, Pin.OUT)
M3_dir  = Pin(22, Pin.OUT)
A1_step = Pin(28, Pin.OUT)
A1_dir  = Pin(27, Pin.OUT)
valve1  = Pin(14, Pin.OUT)
valve2  = Pin(15, Pin.OUT)
buzzer  = Pin(9, Pin.OUT)
up      = Pin(0, Pin.IN, Pin.PULL_UP)
dn      = Pin(1, Pin.IN, Pin.PULL_UP)
left    = Pin(3, Pin.IN, Pin.PULL_UP)
right   = Pin(2, Pin.IN, Pin.PULL_UP)
enter   = Pin(4, Pin.IN, Pin.PULL_UP)
mode    = Pin(5, Pin.IN, Pin.PULL_UP)
pick    = Pin(6, Pin.IN, Pin.PULL_UP)
place   = Pin(7, Pin.IN, Pin.PULL_UP)
za_lim  = Pin(8, Pin.IN, Pin.PULL_UP)
xa_lim  = Pin(10, Pin.IN, Pin.PULL_UP)
xz_lim  = Pin(11, Pin.IN, Pin.PULL_UP)
ya_lim  = Pin(12, Pin.IN, Pin.PULL_UP)
yz_lim  = Pin(13, Pin.IN, Pin.PULL_UP)

M1_step.value(0)
M1_dir.value(0)
M1_step.value(0)
M2_dir.value(0)
M3_step.value(0)
M3_dir.value(0)
A1_step.value(0)
A1_dir.value(0)
valve1.value(0)
valve2.value(0)
buzzer.value(0)

lcd.move_to(0,0)
lcd.putstr("CNC Machine With")
lcd.move_to(0, 1)
lcd.putstr("MicroPython LCD")
time.sleep(1)
lcd.clear()
lcd.putstr("Current Position")
lcd.move_to(0, 1)
lcd.putstr("X=")
lcd.move_to(8, 1)
lcd.putstr(" Y=")
for z in range (0,3):
    buzzer.value(1)
    time.sleep(0.2)
    buzzer.value(0)
    time.sleep(0.2)

x_loc = 0
y_loc = 0
enter_v = 0
up_v = 0
dn_v = 0
right_v = 0
left_v = 0
xa_lim_v = 0
xz_lim_v = 0
ya_lim_v = 0
yz_lim_v = 0
za_lim_v = 0

run_m = 0

def display_x_y():
    lcd.move_to(2,1)
    lcd.putstr("     ")
    lcd.move_to(2,1)
    lcd.putstr(str(x_loc))
    lcd.move_to(11,1)
    lcd.putstr("     ")
    lcd.move_to(11,1)
    lcd.putstr(str(y_loc))

def M3_move(direction, steps):
    M3_dir.value(direction)
    for a in range(steps):
        M3_step.value(1)
        time.sleep_us(1000)
        M3_step.value(0)
        time.sleep_us(1000)
    time.sleep_us(500000)

def A1_dn():
    M3_move(1, 50)

def A1_up():
    M3_move(0, 50)

def A2_dn():
    M3_move(1, 50)

def A2_up():
    M3_move(0, 50)

def A1_pick():
    A1_dn()
    time.sleep(0.25)
    valve1.value(1)
    time.sleep(0.25)
    A1_up()

def A1_place():
    A1_dn()
    time.sleep(0.25)
    valve1.value(0)
    time.sleep(0.5)
    A1_up()

def A2_pick():
    A2_dn()
    time.sleep(0.25)
    valve2.value(1)
    time.sleep(0.25)
    A2_up()

def A2_place():
    A2_dn()
    time.sleep(0.25)
    valve2.value(0)
    time.sleep(0.25)
    A2_up()

def error():
    while True:
        buzzer.value(1)
        time.sleep(1)
        buzzer.value(0)
        time.sleep(0.2)

print("while Loop Start Now")

while True:
    # All Limit Scanning Process 
    xa_lim_v = xa_lim.value()
    xz_lim_v = xz_lim.value()
    ya_lim_v = ya_lim.value()
    yz_lim_v = yz_lim.value()
    Za_lim_v = za_lim.value()

    # All Button Scanning Process
    up_v = up.value()
    dn_v = dn.value()
    left_v = left.value()
    right_v = right.value()
    enter_v = enter.value()
    pick_v = pick.value()
    place_v = place.value()
    run_m = mode.value()

    # When Press Up Button 
    if up_v == 0 and xz_lim_v == 1:
        M1_dir.value(1)
        M1_step.value(1)
        time.sleep_us(1000)
        M1_step.value(0)
        time.sleep_us(1000)
        x_loc += 1
        if xa_lim_v == 0:
            lcd.clear()
            lcd.putstr("Error : 001     MotorX Dir_Wrong")
            error()
        if run_m == 1:
            while up_v == 0:
                up_v = up.value()
                time.sleep_ms(10)

    # when press Dn button
    if dn_v == 0 and xa_lim_v == 1:
        M1_dir.value(0)
        M1_step.value(1)
        time.sleep_us(1000)
        M1_step.value(0)
        time.sleep_us(1000)
        x_loc -= 1
        if xz_lim_v == 0:
            lcd.clear()
            lcd.putstr("Error : 001     MotorX Dir_Wrong")
            error()
        if run_m == 1:
            while dn_v == 0:
                dn_v = dn.value()
                time.sleep_ms(10)

    # When Press left Button 
    if left_v == 0 and ya_lim_v == 1:
        M2_dir.value(0)
        M2_step.value(1)
        time.sleep_us(1000)
        M2_step.value(0)
        time.sleep_us(1000)
        y_loc -= 1
        if yz_lim_v == 0:
            lcd.clear()
            lcd.putstr("Error : 002     MotorY Dir_Wrong")
            error()
        if run_m == 1:
            while left_v == 0:
                left_v = left.value()
                time.sleep_ms(10)

    # when press Right button
    if right_v == 0 and yz_lim_v == 1:
        M2_dir.value(1)
        M2_step.value(1)
        time.sleep_us(1000)
        M2_step.value(0)
        time.sleep_us(1000)
        y_loc += 1
        if ya_lim_v == 0:
            lcd.clear()
            lcd.putstr("Error : 002     MotorY Dir_Wrong")
            error()
        if run_m == 1:
            while right_v == 0:
                right_v = right.value()
                time.sleep_ms(10)

    # When Press Enter
    if enter_v == 0:
        display_x_y()

    # When Press Pick 
    if pick_v == 0:
        if run_m == 0:
            A1_pick()
        if run_m == 1:
            A2_pick()

    #When Press Place
    if place_v == 0:
        if run_m == 0:
            A1_place()
        if run_m == 1:
            A2_place()

    time.sleep_ms(10)