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

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(pin1.value() ^ pin2.value()):
        N1 += 1
    else:
        N1 -= 1


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

pin1.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=ChanA)
pin2.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=ChanB)

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)

        
def derivative(x, dt):
    n = len(x)
    dx = []
    for i in range(0,n):
        dx.append(0)
    for i in range(2,n-1):
        dx[i] = ( x[i+1] - x[i-1] ) / ( 2*dt )
    dx[0] = 2*dx[1] - dx[2]
    dx[n-1] = 2*dx[n-2] - dx[n-3]
    return(dx)

V = 0
dt = 1/20
kv = 65535 / 13.4     # convert volts to pwm
kq = 2*pi/1000        # convert counts to radians
kw = 0.16*pi/4

tmax = 5
npt = int(tmax/dt)

R = []
for i in range(0,20):
    R.append(0)
for i in range(0,20):
    q = 50*(1 - cos(pi*i/20))/2
    R.append(q)
for i in range(0,20):
    q = 50
    R.append(q)
for i in range (0,20):
    q = 50 - 50*(1 - cos(pi*i/20))/2
    R.append(q)
for i in range(0,20):
    q = 0
    R.append(q)
    
dR = derivative(R, dt)
ddR = derivative(dR, dt)

npt = len(R)

Vff = []
for i in range(0,npt):
    V = ddR[i]/39.5 + dR[i]*5/39.5
    Vff.append(V)
    
fwd.duty_u16(0)
rev.duty_u16(0)

Ref = 20
D = 0.0253*6
P = 5*D
E0 = V0 = 0

for i in range(0,npt):
    while(flag == 0):
        pass
    flag = 0

    t = i*dt
    Ref = R[i]
        
    Angle = N1*kq
    Speed = N12*kw

    E1 = E0
    E0 = Ref - Angle
    
    V0 = 0.6065*V0 + 0.5629*(E0 - 0.7789*E1)
    V = V0 + Vff[i]
    
    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(Angle))

 

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