#
# hello.DRV8251A.RP2040.stepper.py
#    H-bridge current regulation 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
from machine import Pin,ADC

machine.freq(250_000_000)
pwm_freq = 250_000_000 # PWM clock frequency
pwm_range = (1<<10)-1 # PWM range
m = int(0.7*pwm_range) # max PWM value
nstep = 50 # number of steps to take
tstep = 40000 # full step time (us)
A1_pin = 27 # D1
A2_pin = 28 # D2
B1_pin = 7 # D5
B2_pin = 0 # D6
VA_pin = 26 # D0
VB_pin = 6 # D4
IA_pin = 29 # D3

@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_2 = calc_steps(1)
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

VA = pwm_pio(1,VA_pin,max_count=pwm_range,count_freq=pwm_freq)
VB = pwm_pio(2,VB_pin,max_count=pwm_range,count_freq=pwm_freq)

VA.put(-1)
VB.put(-1)

A1 = Pin(A1_pin,Pin.OUT)
A1.value(0)
A2 = Pin(A2_pin,Pin.OUT)
A2.value(0)
B1 = Pin(B1_pin,Pin.OUT)
B1.value(0)
B2 = Pin(B2_pin,Pin.OUT)
B2.value(0)

IA = ADC(Pin(IA_pin))

@micropython.native
def do_step(step,duration,table):
    stepA = table[0][step]
    stepB = table[1][step]
    if (stepA > 0):
        A1.value(1)
        A2.value(0)
        VA.put(stepA)
    else:
        A1.value(0)
        A2.value(1)
        VA.put(-stepA)
    if (stepB > 0):
        B1.value(1)
        B2.value(0)
        VB.put(stepB)
    else:
        B1.value(0)
        B2.value(1)
        VB.put(-stepB)
    time.sleep_us(duration)
    return IA.read_u16()
    
@micropython.native
def do_steps(table):
    length = len(table[0])
    duration = tstep//length
    steps = nstep*length
    current = 0
    for step in range(steps):
        current += do_step(step%length,duration,table)
    for step in range(steps,0,-1):
        current += do_step(step%length,duration,table)
    current = (3.3*(current/(2*steps))/65535)/(1575e-6*499)
    return current

while True:
    print('1/2 step',end='')
    print(f" {do_steps(step_2):.2f}A")
    print('1/4 step',end='')
    print(f" {do_steps(step_4):.2f}A")
    print('1/8 step',end='')
    print(f" {do_steps(step_8):.2f}A")
    print('1/16 step',end='')
    print(f" {do_steps(step_16):.2f}A")
    print('1/32 step',end='')
    print(f" {do_steps(step_32):.2f}A")