main.py:  master controller file

import os

import math

import cv2

import numpy as np

import RPi.GPIO as GPIO

from motor import Servo

def get_ball_coordinates(filename='frame.jpg'):

    # initialize webcdddam capture

    # in order (hue, sat, val)

    lows = [30, 50, 20]

    highs = [80, 255, 255]

    bgr_img = cv2.imread(filename)

    thresh_img = get_threshold_img(bgr_img, lows, highs)

    # morphological operations:

    ksize = 5

    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (ksize, ksize))

    # closing and opening:

    filtered_img = cv2.morphologyEx(thresh_img, cv2.MORPH_CLOSE, kernel)

    filtered_img = cv2.morphologyEx(filtered_img, cv2.MORPH_OPEN, kernel)

    nb_components, output, stats, centroids = cv2.connectedComponentsWithStats(filtered_img)

    areas = stats[:, cv2.CC_STAT_AREA]

    if centroids.shape[0] > 1:

        centroids = centroids[1:]

        areas = areas[1:]

        max_area = areas[0]

        largest_centroid = centroids[0]

        largest_centroid_index = 0

        for detected_object in range(nb_components - 1):

            if areas[detected_object] > max_area:

                largest_centroid_index = detected_object

                max_area = areas[detected_object]

                largest_centroid = centroids[detected_object]

        center = largest_centroid

        return center[0], center[1]

    else:

        return None

def get_threshold_img(bgr_img, lows, highs):

    # convert to HSV:

    img = cv2.cvtColor(bgr_img, cv2.COLOR_BGR2HSV)

    # split hsv image into Hue, Sat, and Value

    planes = cv2.split(img)

    image_height = img.shape[0]

    image_width = img.shape[1]

    # create blank image with all white pixels

    thresh_img = np.full((image_height, image_width), 255, dtype=np.uint8)

    # create hue, saturation, and value images using low and high thresholds

    for i in range(3):

        _, low_img = cv2.threshold(planes[i], lows[i], 255, cv2.THRESH_BINARY)

        _, high_img = cv2.threshold(planes[i], highs[i], 255, cv2.THRESH_BINARY_INV)

        # use bitwise and to combine hue, sat, and val into one image

        thresh_band_img = cv2.bitwise_and(low_img, high_img)

        thresh_img = cv2.bitwise_and(thresh_img, thresh_band_img)

    return thresh_img

def take_pic():

    if os.path.exists('frame.jpg'):

        os.remove('frame.jpg')

    url = f"http://192.168.35.118:4747/cam/1/frame.jpg"

    os.system(f"wget {url} >/dev/null 2>&1")

def get_servos():

    ne = Servo(10, 'ne')

    nw = Servo(11, 'nw')

    se = Servo(12, 'se')

    sw = Servo(13, 'sw')

    servos = [nw, ne, sw, se]

    return servos

def calibrate():

    take_pic()

    coords = get_ball_coordinates()

    if coords is None:

        return calibrate()

    return coords

def get_axis_err(current_coords, origin_coords):

    dx = current_coords[0] - origin_coords[0]

    dy = current_coords[1] - origin_coords[1]

    dy *= -1

    angle = math.radians(45)

    scalar = math.cos(angle)

    errx = dx*scalar - dy*scalar

    erry = dx*scalar + dy*scalar

    return errx, erry

def err2pct(err, prev_err=None):

    d_weight = 1.85

    errx, erry = err

    if prev_err is not None:

        # Derivative control

        prev_errx, prev_erry = prev_err

        d_errx = errx - prev_errx

        d_erry = erry - prev_erry

        errx += d_weight*d_errx

        erry += d_weight*d_erry

    nw_pct = -1*errx

    ne_pct = erry

    sw_pct = -1*erry

    se_pct = 1*errx

    result = [nw_pct, ne_pct, sw_pct, se_pct]

    return result

if __name__ == '__main__':

    verbose = 0

    prev_err = None

    try:

        servos = get_servos()

        for servo in servos:

            servo.setup()

        input('Press enter to calibrate')

        origin = calibrate()

        while True:

            take_pic()

            c = get_ball_coordinates()

            if c is None:

                continue

            errx, erry = get_axis_err(c, origin)

            cv2.waitKey(1)

            pcts = err2pct((errx, erry), prev_err)

            for i,pct in enumerate(pcts):

                servo = servos[i]

                dc = servo.pct2dc(pct)

                servo.set_dc(dc)

            prev_err = errx, erry

            if verbose > 0:

                img = cv2.imread('frame.jpg')

                cv2.drawMarker(img, tuple(map(round, origin)), (0,0,255), cv2.MARKER_CROSS, 20, 1)

                cv2.drawMarker(img, tuple(map(round, c)), (0,0,255), cv2.MARKER_CROSS, 20, 1)

                cv2.line(img, tuple(map(round, origin)), tuple(map(round, c)), (0,0,255), 2)

                cv2.imshow('image', img)

    except KeyboardInterrupt:

        print("Program Terminated")

    finally:

        GPIO.cleanup()