#https://www.youtube.com/watch?v=FJwgDaSsob8
import math
from machine import Pin,PWM,Timer

#pin for Motor Encoder
encm_PA = Pin(2, Pin.IN, Pin.PULL_UP)
encm_PB = Pin(3, Pin.IN, Pin.PULL_UP)
#pin for setpoint encoder
encs_PA = Pin(4, Pin.IN, Pin.PULL_UP)
encs_PB = Pin(5, Pin.IN, Pin.PULL_UP)
#define pin in1 and in2 for h-brige
in1=Pin(6, Pin.OUT)
in2=Pin(7, Pin.OUT)
#create a PWM object pem comes out pin 14
pwm_out= PWM(Pin(8))
pwm_out.freq(50)
#
#pin to check sampling frequence in pin 21 Should be 500Hz
sample_pin = Pin(9, Pin.OUT)
#
#Hardware timer interrupt open an instance
timer0= Timer()
interruptCounter=0
#timer interrupt handler set for 1khz(1ms)
def tick(timer):
	global interruptCounter
	interruptCounter = interruptCounter+1
timer0.init(freq=1000,mode=Timer.PERIODIC,callback=tick)
#isd for motor quad encoder
counter_motor = 0
def handler_motor(pin_m):
	global counter_motor #note must be globa here
	if  encm_PA.value():
		counter_motor -= 1
	else:
		counter_motor += 1
		#print("motor enoder counter:", counter_motor)
		#print("pin (object) that triggered this motor hadler is :", pin_m)
encm_PB.irq(handler_motor, Pin.IRQ_FALLING)

#ISR for motor quad setpoint encoder
counter_setpoint = 0
def handler_setpoint(pin_s):
	global counter_setpoint #note musb be global here
	if encs_PA.value():
		counter_setpoint -=1
	else:
		counter_setpoint +=1
		#print("setpoint enoder counter:", counter_motor)
		#print("pin (object) that triggered this motor hadler is :", pin_s)
encs_PB.irq(handler_setpoint,Pin.IRQ_FALLING)
###
#Find the low pass digital filter terms
Ts = 1e-3 # 1ms sampling interval, 1khz samling freq
fc = 10 #cutoff frequency
def lowpass(Ts,fc):
	T=1/(2*math.pi*fc)  #timer constans of filter
	kf=Ts/(Ts+2*T) #gain
	af = (Ts-2*T)/(Ts+2*T) #a parameter in denominator
	#Difference equation is a below
	#y =-af*y1+Kf(u+u1)
	#return the value as a list
	values=[kf,af]
	return values
values= lowpass(Ts,fc) #find lowpass digital filter calues
print("Gian (kf) and af parameter are", values)
kf=values[0]
af=values[1]
y1=0
y=0
u=0
u1=0

interruptCounter = 0

#init sample pin to true
sample_pin.value(1)
#stop motor initially
in1.value(0)
in2.value(0)
#initialise values
I_term = 0.0
I_term_old = 0.0
error= 0
error_old= 0
#gains for proportional and derivate
Kp=500
Kd=100
Ki=0.1
#loop forever
while(1):
	if interruptCounter>0:
		interruptCounter=interruptCounter-1
		#countinue code hire within while loop
		#store last value of error
		errorold=error
		error=counter_setpoint-counter_motor
		
		if error>0:
			in1.value(0)
			in2.value(1)
		else:
			in1.value(1)
			in2.value(0)
		#propsitional control
		P_term=error*Kp
		#derivate term
		D_term=(error-errorold)*Kd
		#integral term
		I_term = 0.999*I_term_old+error*Ki
		#PID is the sum od the tree term
		PID=P_term+D_term+I_term
			
		#lowpass filter
		u1=u
		u= PID #take PID output and filter it first
		#first ordes difference equation of a lowpass filter
		y1=y
		y=af*y1+kf*(u+u1)
		pwm_out.duty_u16(abs(int(y)))
	#toggle logic out to measure sampling rate
	#dont need this in final imlemantation of course
		#if sample_pin.value():
		#	sample_pin.value(0)
		#else:
		#	sample_pin.value(1)