import machine
from machine import Pin
from time import sleep
from neopixel import NeoPixel
from motion_controller.accel_stepper import AccelStepper, DRIVER
from utime import ticks_us, sleep_us

"""CL57T(V4.0) Closed-Loop Stepper Driver
10
Figure 10: Sequence chart of control signals
Remark:
a) t1: ENA must be ahead of DIR by at least 200ms. Usually, ENA+ and ENA- are NC (not connected). See
“Connector P1 Configurations” for more information.
b) t2: DIR must be ahead of PUL effective edge by 2us to ensure correct direction;
c) t3: Pulse width not less than 1us;
d) t4: Low level width not less than 1us;
e) Duty cycle of PUL signal is recommended 50%"""

def machine_main():
    # machine.freq()  # get the current frequency of the CPU
    # machine.freq(240000000)  # set the CPU frequency to 240 MHz

    thrust_step_pin = Pin("D10", Pin.OUT)
    thrust_dir_pin = Pin("D9", Pin.OUT)
    thrust_step_pin.value(0)
    thrust_dir_pin.value(0)

    def step():
        thrust_step_pin.value(1)
        sleep_us(1)
        thrust_step_pin.value(0)

    def fwd():
        thrust_dir_pin.value(1)
        sleep_us(2)
        step()

    def rev():
        thrust_dir_pin.value(0)
        sleep_us(2)
        step()

    thrust_axis = AccelStepper(fwd, rev)
    thrust_axis_endstop_estop_pin = Pin("D21", Pin.IN, Pin.PULL_UP)

    pin = Pin("D6", Pin.OUT)  # Pin number for v1 of the above DevKitC, use pin 38 for v1.1
    np = NeoPixel(pin, 3)  # "1" = one RGB led on the "led bus"
    for pixel in range(3):
        np[pixel] = (0, 255, 0)
        np.write()

    thrust_steps_per_revolution = 3200
    thrust_max_speed = 20 * thrust_steps_per_revolution

    thrust_homing_speed = 8 * thrust_steps_per_revolution
    thrust_max_acceleration = 5 * thrust_steps_per_revolution

    thrust_axis.set_max_speed(thrust_max_speed)
    thrust_axis.set_acceleration(thrust_max_acceleration)
    thrust_axis.set_speed(thrust_homing_speed)

    thrust_axis.move(-100*thrust_steps_per_revolution)

    print("Homing")
    while thrust_axis_endstop_estop_pin.value():
        thrust_axis.run()
        print(f"{thrust_axis.current_position()}:{thrust_axis.target_position()}")
    print("Homed")
    thrust_axis.set_current_position(0)

    thrust_axis.set_max_speed(thrust_max_speed)
    thrust_axis.run_to_new_position(200)

    for i in range(3):
        thrust_axis.run_to_new_position(5000)
        thrust_axis.run_to_new_position(200)



machine_main()