import RPi.GPIO as GPIO
from time import sleep
import pigpio

pi = pigpio.pi()

PWM1 = 16 #motor1 snelheid 
PWM2 = 20 #motor2 snelheid   
INA1 = 5 #richting a motor 1
INB1 = 6 #richting b motor 1
INA2 = 23 #richting a motor 2
INB2 = 24 #richting b motor 2

running = True
minsnelheid = 12

GPIO.setwarnings(False)#gpio waarschuwingen uit
GPIO.setmode(GPIO.BCM)#pinmode instellen
GPIO.setup(PWM1, GPIO.OUT)#instellen als in/uitgangen
GPIO.setup(PWM2, GPIO.OUT)
GPIO.setup(INA1, GPIO.OUT)
GPIO.setup(INB1, GPIO.OUT)
GPIO.setup(INA2, GPIO.OUT)
GPIO.setup(INB2, GPIO.OUT)

pi.set_mode(PWM1, pigpio.OUTPUT)
pi.set_mode(PWM2, pigpio.OUTPUT)
pi.set_PWM_frequency(PWM1, 100)
pi.set_PWM_frequency(PWM2, 100)

def MOTOR1(richting1, snelheid1):
    if richting1 == 1:  #motor1 rechts
        GPIO.output(INA1, 1) #ina op 1 instellen inb op 0, zodat motor naar rechts draait
        GPIO.output(INB1, 0)
        sleep(0.01) #wacht 10ms
        if snelheid1 >= minsnelheid: #beveiliging voor te lage snelheid
            pi.set_PWM_dutycycle(PWM1, snelheid1)
        print('Rechts M1') #print snelheid
    if richting1 == 0:  #motor1 links
        GPIO.output(INA1, 0)
        GPIO.output(INB1, 1)
        sleep(0.01)
        if snelheid1 >= minsnelheid:
            pi.set_PWM_dutycycle(PWM1, snelheid1)
        print('Links M1') 

def MOTOR2(richting2, snelheid2):
    if richting2 == 1: #motor2 rechts
        GPIO.output(INA2, 1)
        GPIO.output(INB2, 0)
        sleep(0.01)
        if snelheid2 >= minsnelheid:
            pi.set_PWM_dutycycle(PWM2, snelheid2)
        print('Rechts M2')
    if richting2 == 0:  #motor2 links
        GPIO.output(INA2, 0)
        GPIO.output(INB2, 1)
        sleep(0.01)
        if snelheid2 >= minsnelheid:
            pi.set_PWM_dutycycle(PWM2, snelheid2)
        print('Links M2')

def main():
    MOTOR1(0, 255)#fucntie aanroepen met richting en snelheid
    MOTOR2(0, 50)

try:
    while True:
        main()
    
except KeyboardInterrupt: #stop als knop word ingedrukt
    print("Exit Button pressed")

finally:
    print('exit1')
    pi.set_PWM_dutycycle(PWM1, 0)
    pi.set_PWM_dutycycle(PWM2, 0)
    GPIO.cleanup() #pins resseten
        
print('exit2')