#https://www.youtube.com/watch?v=-J_agSsb5yo
#proportional pluss difference control
#Micropythonon an ESP32
#load the modules
import machine,time,esp32
from machine import Pin
from machine import Timer
#########
#Pin for motor encoder
encm_PA = Pin(25,Pin.IN, Pin.PULL_UP)
encm_PB = Pin(33,Pin.IN, Pin.PULL_UP)
#isr for motor quad encoder
counter_motor=0
def handler(c):
    global counter_motor
    if encm_PA.value():
        counter_motor -=1
    else:
        counter_motor +=1
#		print("motor encoder counter:" counter_motor)
encm_PB.irq(handler,Pin.IRQ_FALLING)
#################
#pins for setpint encoder
encs_PA =Pin(32, Pin.IN, Pin.PULL_UP)
encs_PB =Pin(35, Pin.IN, Pin.PULL_UP)

#ISR for motor quad encoder
counter_setpoint =0
def hadler(c):
    global counter_setpoint
    if encs_PA.value():
        counter_setpoint -= 1
    else:
        counter_setpoint += 1
#		print("Setpoint Encoder counter:",counter_setpoint)

encs_PB.irq(hadler, Pin.IRQ_FALLING)
#############
#create a PWM object . pwm comes out
pwm_out = machine.PWM(machine.Pin(14),freq=50)
#define pins in1 and in2 for h-brige
in1= Pin(27, Pin.OUT)
in2= Pin(26, Pin.OUT)
#pin to check sample frequency is pin21
sample_pin=Pin(21,Pin.OUT)

interruptCounter = 0
#int a timer called 0

timer0 = machine.Timer(0)
def hadleInterrup(timer):
    global interruptCounter
    interruptCounter += 1#= interruptCounter +1
#set the period of the timer
timer0.init(period=1, mode=machine.Timer.PERIODIC, callback=hadleInterrup)

sample_pin.value(1)
#stop motor initially
in1.value(0)
in2.value(0)
pwm_out.duty(1000)

error=0
#Gains
Kp=10
Kd=20
#loop forever
print("Servo Drive Start")
while(1):
    if interruptCounter>0:
        interruptCounter -=1 #interruptCounter-1
        #continue code here within while loop
        #store last calue 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)
        #proportional control
        P_term=error*Kp
        #Derivative term
        D_term =(error-errorold)*Kd
        PID=P_term+D_term
        pwm_out.duty(abs(PID))
        #Toggle logic out to measure sample
        if sample_pin.value():
            sample_pin.value(0)
        else:
            sample_pin.value(1)