#
# hello.DRV8421A.RP2040.stepper.py
#    dual H-bridge stepper hello-world
#
# Neil Gershenfeld 11/15/25
#
# This work may be reproduced, modified, distributed,
# performed, and displayed for any purpose, but must
# acknowledge this project. Copyright is retained and
# must be preserved. The work is provided as is; no
# warranty is provided, and users accept all liability.
#
# install MicroPython
#    https://micropython.org/download/RPI_PICO/
#

from rp2 import PIO,StateMachine,asm_pio
import time,math,machine,array

machine.freq(250_000_000)
pwm_freq = 250_000_000 # PWM clock frequency
pwm_range = (1<<13)-1 # PWM range
m = int(0.65*pwm_range) # max PWM value
nstep = 50 # number of steps to take
tstep = 40000 # full step time (us)
I1 = 3 # D10
I2 = 4 # D9
I3 = 2 # D8
I4 = 1 # D7

step_1 = [[m, m,-m,-m],
          [m,-m,-m, m]]
step_2 = [[m,m, m, 0,-m,-m,-m,0],
          [m,0,-m,-m,-m, 0, m,m]]

@micropython.native
def calc_steps(power):
    length = int(math.pow(2,power+2))
    segment = length//4
    steps = [array.array('i',[0]*length),array.array('i',[0]*length)]
    for i in range(0,segment):
        steps[0][i] = m
        steps[1][i] = int(m*((segment-1-i)/(segment-1))+(-m)*i/(segment-1))
        steps[0][segment+i] = int(m*((segment-1-i)/(segment-1))+(-m)*i/(segment-1))
        steps[1][segment+i] = -m
        steps[0][2*segment+i] = -m
        steps[1][2*segment+i] = int((-m)*((segment-1-i)/(segment-1))+m*i/(segment-1))
        steps[0][3*segment+i] = int((-m)*((segment-1-i)/(segment-1))+m*i/(segment-1))
        steps[1][3*segment+i] = m
    return steps

step_4 = calc_steps(2)
step_8 = calc_steps(3)
step_16 = calc_steps(4)
step_32 = calc_steps(5)

@asm_pio(sideset_init=PIO.OUT_LOW)
def pwm_prog():
    pull(noblock).side(0)
    mov(x,osr)
    mov(y,isr)
    label("loop")
    jmp(x_not_y,"skip")
    nop().side(1)
    label("skip")
    jmp(y_dec,"loop")

class pwm_pio:
    def __init__(self,sm_id,pin,max_count,count_freq):
        self._sm = StateMachine(sm_id,pwm_prog,freq=count_freq,sideset_base=machine.Pin(pin))
        self._sm.put(max_count)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr,osr)")
        self._sm.active(1)
        self._max_count = max_count
        self.put = self._sm.put

pwm1 = pwm_pio(0,I1,max_count=pwm_range,count_freq=pwm_freq)
pwm2 = pwm_pio(1,I2,max_count=pwm_range,count_freq=pwm_freq)
pwm3 = pwm_pio(2,I3,max_count=pwm_range,count_freq=pwm_freq)
pwm4 = pwm_pio(3,I4,max_count=pwm_range,count_freq=pwm_freq)

pwm1.put(-1)
pwm2.put(-1)
pwm3.put(-1)
pwm4.put(-1)

@micropython.native
def do_step(step,duration,table):
    step12 = table[0][step]
    step34 = table[1][step]
    if (step12 > 0):
        pwm1.put(step12)
        pwm2.put(-1)
    else:
        pwm1.put(-1)
        pwm2.put(-step12)
    if (step34 > 0):
        pwm3.put(step34)
        pwm4.put(-1)
    else:
        pwm3.put(-1)
        pwm4.put(-step34)
    time.sleep_us(duration)

@micropython.native
def do_steps(table):
    length = len(table[0])
    duration = tstep//length
    steps = nstep*length
    for step in range(steps):
        do_step(step%length,duration,table)
    for step in range(steps,0,-1):
        do_step(step%length,duration,table)

while True:
    print('full step')
    do_steps(step_1)
    print('1/2 step')
    do_steps(step_2)
    print('1/4 step')
    do_steps(step_4)
    print('1/8 step')
    do_steps(step_8)
    print('1/16 step')
    do_steps(step_16)
    print('1/32 step')
    do_steps(step_32)