# I Control for a DC Servo Motor

from machine import Pin, PWM, Timer
from time import sleep
from math import sin, pi, floor

Out1 = Pin(18, Pin.OUT)
Out2 = Pin(19, Pin.OUT)

fwd = PWM(Pin(18))
rev = PWM(Pin(19))

fwd.freq(100)
rev.freq(100)

pin1 = Pin(26,Pin.IN)
pin2 = Pin(27,Pin.IN,Pin.PULL_UP)
N1 = N2 = N12 = 0
flag = 0

def ChanA(pin1):
    global pin2
    global N1
    if(pin2.value()):
        N1 -= 1
    else:
        N1 += 1

pin1.irq(trigger=Pin.IRQ_RISING, handler=ChanA)

Speed = E1 = E0 = Vin = flag = Ref = 0

def tick(timer):
    global N1, N2, N12, flag
    X = N1
    N12 = N1 - N2
    N2 = X
    flag = 1
        
tim = Timer()
tim.init(freq=20, mode=Timer.PERIODIC, callback=tick)

t = V = 0
dt = 1/20
kv = 65535 / 13.4   # convert volts to pwm
kw = 0.16*pi        # convert counts to rad/sec

fwd.duty_u16(0)
rev.duty_u16(0)

while(t < 10):
    while(flag == 0):
        pass
    flag = 0
    Ref = floor(t/5) * 40 + 40
    
    Speed = kw * N12
    dV = 0.159*(Ref - Speed)
    V += dV * dt
    
    if(V > 0):
        fwd.duty_u16(int(V*kv))
        rev.duty_u16(0)
    else:
        fwd.duty_u16(0)
        rev.duty_u16(int(-V*kv))
       
    print('{: 7.2f}'.format(t), '{: 7.2f}'.format(Ref), '{: 7.4f}'.format(Speed))
    t += dt
 

print('Stop')
fwd.duty_u16(0)
rev.duty_u16(0)