#PlattenspielerMotoransteuerung




#Parameter Wert
#PWM 20–40 kHz
# Control loop 10–20 kHz
# ADC 80 kHz
# Deadtime im IR2184

from machine import Pin
import rp2
import time
import math
import math
import gc
gc.disable()

global event1
event1 = 0
global event2
event2 = 0
@rp2.asm_pio()
def button_irq1():
    wrap_target()
    
    in_(pins, 1)
    mov(x, isr)
    
    label("wait_change")
    in_(pins, 1)
    mov(y, isr)
    
    jmp(x_not_y, "debounce")
    jmp("wait_change")
    
    # Entprell-Phase
    label("debounce")
    set(y, 5)          # Anzahl Prüfzyklen
    
    label("debounce_loop")
    in_(pins, 1)
    jmp(y_dec, "debounce_loop")

    label("changed")
    mov(x, isr)
    jmp(not_x, "do_irq")   # nur wenn 0 (bei PULL_UP → gedrückt)
    jmp("wait_change")
    
    label("do_irq")
    irq(0)

    wrap()

def handler(sm1):
    global event1
    event1 += 1


@rp2.asm_pio()
def button_irq2():
    wrap_target()
    
    in_(pins, 1)
    mov(x, isr)
    
    label("wait_change")
    in_(pins, 1)
    mov(y, isr)
    
    jmp(x_not_y, "debounce")
    jmp("wait_change")
    
    # Entprell-Phase
    label("debounce")
    set(y, 5)          # Anzahl Prüfzyklen
    
    label("debounce_loop")
    in_(pins, 1)
    jmp(y_dec, "debounce_loop")

    label("changed")
    mov(x, isr)
    jmp(not_x, "do_irq") 
    jmp("wait_change")

    label("do_irq")
    irq(1)
    
    wrap()

def handler2(sm2):
    global event2
    event2 += 1
#    print("Button event2!", event2)


button = Pin(1, Pin.IN, Pin.PULL_UP)
button2 = Pin(2, Pin.IN, Pin.PULL_UP)

sm1 = rp2.StateMachine(0, button_irq1, freq=20000, in_base=button)
sm2 = rp2.StateMachine(1, button_irq2, freq=20000, in_base=button2)
sm1.irq(handler)
sm1.active(1)
sm2.irq(handler2)
sm2.active(1)



#PWM



PERIOD = 40
TABLE_SIZE = 104

pwm_table = []

for i in range(TABLE_SIZE):
    duty = 100 * (0.5 * math.sin(i * 7) + 0.5)

    high = round((duty * PERIOD) / 100)
    low = PERIOD - high
    
    pwm_table.append((high << 16) | low)


@rp2.asm_pio(set_init=rp2.PIO.OUT_LOW)
def pwm_prog():
    wrap_target()

    pull(block)
    out(x, 16)
    out(y, 16)

    set(pins, 1)
    label("high")
    jmp(x_dec, "high")

    set(pins, 0)
    label("low")
    jmp(y_dec, "low")

    wrap()

sm5 = rp2.StateMachine(5, pwm_prog, freq=1000000, set_base=Pin(16))
sm5.active(1)

index = 0

for _ in range(8):
    sm5.put(pwm_table[index])
    index = (index + 1) % len(pwm_table)


#
#    for _ in range(8):
#        sm.put(pwm_table[index])
#        index = (index + 1) % len(pwm_table)

while True:
    for _ in range(8):
        index = (phase // SCALE) % TABLE_SIZE
        sm5.put(pwm_table[index])

        phase += freq



       
phase = 0
freq = 18
SCALE = 10000

target_speed = 33.33
current_speed = 0

control = 0   # Stellgröße


#P-Regler
error = target_speed - current_speed
control += 0.1 * error

#Umrechnung in PWM
duty = max(0, min(100, control))

high = int((duty * PERIOD) / 100)
low = PERIOD - high

value = (high << 16) | low

#PWM-Update (entkoppelt!)
while True:
    for _ in range(8):
        sm5.put(value)

#Bessere Version mit Phase (dein aktuelles System)
phase = 0

# “echte” Phase statt Periodendauer
phase_error = (expected_edge_time - actual_edge_time)


while True:

    # 🔘 Buttons verarbeiten
    if event1:
        event1 = 0
        freq += 1

    if event2:
        event2 = 0
        freq -= 1

    # ⚙️ PWM erzeugen
    for _ in range(8):
        index = (phase // SCALE) % TABLE_SIZE
        sm5.put(pwm_table[index])
        phase += freq

# Zeitgesteuerte Regelung
last = time.ticks_us()

while True:
    now = time.ticks_us()

    if time.ticks_diff(now, last) > 1000:   # 1 kHz Regelung
        last = now

        # Regler hier






while True:

    if event1:
        event1 = 0
        print("Button 1 gedrückt")

        # 👉 Beispiel: Frequenz erhöhen
        freq += 1

    if event2:
        event2 = 0
        print("Button 2 gedrückt")

        # 👉 Beispiel: Frequenz verringern
        freq -= 1