maxtracker

#memanggil library yang diperlukan
import sys
import time
import RPi.GPIO as GPIO
import cv2
import numpy as np

#memulai kamera
cap = cv2.VideoCapture(0)

cap.set(3, 640) # width
cap.set(4, 480) # height

#mereset GPIO agar siap digunakan
GPIO.cleanup()

#menggunakan BCM sebagai referensi GPIO
GPIO.setmode(GPIO.BCM)

#mengatur GPIO untuk keluaran
Mot1F=24
Mot1R=25
Mot2F=18
Mot2R=23

GPIO.setup(Mot1F, GPIO.OUT)
GPIO.setup(Mot1R, GPIO.OUT)
GPIO.setup(Mot2F, GPIO.OUT)
GPIO.setup(Mot2R, GPIO.OUT)

#mengatur GPIO masukan
GPIO.setup(16, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(12, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(7, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(8, GPIO.IN, pull_up_down=GPIO.PUD_UP)

Lim1=GPIO.input(16)
Lim2=GPIO.input(12)
Lim3=GPIO.input(8)
Lim4=GPIO.input(7) #kanan

#membuat fungsi-fungsi yang diperlukan

#fungsi motor dengan pwm software menggunakan frekuensi 50Hz
def motor1forward(dur, cycle):
    t1=0.02
    a1=cycle/100
    ton=a1*t1
    toff=t1-ton
    t4 = int(round(50*dur))
    print("forwarding motor 1")
    for i in range (1, t4) :
        GPIO.output(Mot1F, GPIO.HIGH)
        time.sleep(ton)
        GPIO.output(Mot1F, GPIO.LOW)
        time.sleep(toff)
    del dur, cycle, t1, a1, ton, toff, t4

def motor1reverse(dur, cycle):
    t1=0.02
    a1=cycle/100
    ton=a1*t1
    toff=t1-ton
    t4 = int(round(50*dur))
    print("backwarding motor 1")
    for i in range (1, t4) :
        GPIO.output(Mot1R, GPIO.HIGH)
        time.sleep(ton)
        GPIO.output(Mot1R, GPIO.LOW)
        time.sleep(toff)
    del dur, cycle, t1, a1, ton, toff, t4

def motor2forward(dur, cycle):
    t1=0.02
    a1=cycle/100
    ton=a1*t1
    toff=t1-ton
    t4 = int(round(50*dur))
    print("forwarding motor 2")
    for i in range (1, t4) :
        GPIO.output(Mot2F, GPIO.HIGH)
        time.sleep(ton)
        GPIO.output(Mot2F, GPIO.LOW)
        time.sleep(toff)
    del dur, cycle, t1, a1, ton, toff, t4

def motor2reverse(dur, cycle):
    t1=0.02
    a1=cycle/100
    ton=a1*t1
    toff=t1-ton
    t4 = int(round(50*dur))
    print("backwarding motor 2")
    for i in range (1, t4) :
        GPIO.output(Mot2R, GPIO.HIGH)
        time.sleep(ton)
        GPIO.output(Mot2R, GPIO.LOW)
        time.sleep(toff)
    del dur, cycle, t1, a1, ton, toff, t4


while True: #infinite looping agar program terus berjalan

    _, frame = cap.read()

    #orig = frame.copy()

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    gray = cv2.GaussianBlur(gray,(161,161),0)
    (minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(gray)

    image = frame.copy()
    cv2.circle(image, maxLoc, 161, (255, 0, 0), 2)
    cv2.imshow("Alhamdulillah", image)

    centre_x, centre_y = (int(x) for x in maxLoc)
    print (centre_x, centre_y)

    if 280<centre_x<360:
        print("motor1 stop")
    if 200<centre_y<280:
        print("motor2 stop")

    if (80<centre_x<280):
        if (Lim4 == True):
            print("motor 1 kekanan pelan")
            motor1reverse(.250, 20)
        if (Lim4 == False):
            print("motor 1 maksimal")

    if (80<centre_y<200):
        if (Lim1 == True):
            print("motor 2 bawah pelan")
            motor2forward(.250, 80)
        if (Lim1 == False):
            print("motor 2 maksimal")

    if (360<centre_x<560):
        if (Lim3 == True):
            print("motor 1 kiri pelan")
            motor1forward(.250, 20)
        if (Lim1 == False):
            print("motor 1 maksimal")

    if (280<centre_y<400):
        if (Lim2 == True):
            print("motor 2 keatas pelan")
            motor2forward(.250, 80)
        if (Lim2 == False):
            print("motor 2 maksimal")

    if (centre_x<80):
        if (Lim4 == True):
            print("motor 1 kekanan fullspeed")
            motor1forward(.250, 21)
        if (Lim4 == False):
            print("motor 1 maksimal")

    if (centre_y<80):
        if (Lim1 == True):
            print("motor 2 bawah fullspeed")
            motor2forward(.250, 100)
        if (Lim1 == False):
            print("motor 2 maksimal")

    if (560<centre_x):
        if (Lim3 == True):
            print("motor 1 kiri fullspeed")
            motor1reverse(.250, 21)
        if (Lim1 == False):
            print("motor 1 maksimal")

    if (400<centre_y):
        if (Lim2 == True):
            print("motor 2 keatas fullspeed")
            motor1forward(.250, 100)
        if (Lim2 == False):
            print("motor 2 maksimal")

    k = cv2.waitKey(5) & 0xFF
    if k == 27:
        break

cv2.destroyAllWindows()
del cap