t = V = I = 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
    E = Ref - Speed
    I += E*dt
    V = 0.254*E + 1.272*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(Speed))
    t += dt
 

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