Обнаружение Aruco обнаруживает маркеры, двигатель D C продолжает работать, несмотря на вход GPIO - PullRequest
Новая
       23

Обнаружение Aruco обнаруживает маркеры, двигатель D C продолжает работать, несмотря на вход GPIO

0 голосов
/ 11 июля 2020

Недавно я занялся программированием и решил заняться проектом Aruco, чтобы узнать больше о компьютерном зрении. Я придумал дизайн робота, который соответствует маркеру Aruco с использованием двух независимых двигателей D C. Проводка для GPIO и драйвера двигателя (L293D) протестирована с помощью другого скрипта и работает. Мой план на текущем этапе состоял в том, чтобы активировать два двигателя, когда сценарий обнаруживает допустимый маркер и останавливает двигатель, когда маркер не обнаружен или не исчез. , один из двигателей D C продолжал работать, несмотря на низкий уровень входного сигнала GPIO. Ниже мой сценарий: 

import glob
import cv2
import cv2.aruco as aruco
import numpy as np
import RPi.GPIO as GPIO
from time import sleep

GPIO.setwarnings(False)

GPIO.setmode(GPIO.BOARD)

Motor1A = 16
Motor1B = 18
Motor1E = 22

Motor2A = 19
Motor2B = 21
Motor2E = 23

GPIO.setup(Motor1A, GPIO.OUT)
GPIO.setup(Motor1B, GPIO.OUT)
GPIO.setup(Motor1E, GPIO.OUT)

GPIO.setup(Motor2A, GPIO.OUT)
GPIO.setup(Motor2B, GPIO.OUT)
GPIO.setup(Motor2E, GPIO.OUT)

pwm = GPIO.PWM(22, 100)
pwm = GPIO.PWM(23, 100)

cap = cv2.VideoCapture(0)
center_of_frame = 640/2

####---------------------- CALIBRATION ---------------------------
# termination criteria for the iterative algorithm
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
# checkerboard of size (7 x 6) is used
objp = np.zeros((6*7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)

# arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.

# iterating through all calibration images
# in the folder
images = glob.glob('calib/*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # find the chess board (calibration pattern) corners
    ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)

    # if calibration pattern is found, add object points,
    # image points (after refining them)
    if ret == True:
        objpoints.append(objp)

        # Refine the corners of the detected corners
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners2)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (7, 6), corners2, ret)


ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)


###------------------ ARUCO TRACKER ---------------------------
while True:
    ret, frame = cap.read()

    # operations on the frame
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # set dictionary size depending on the aruco marker selected
    aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)

    # detector parameters can be set here (List of detection parameters[3])
    parameters = aruco.DetectorParameters_create()
    parameters.adaptiveThreshConstant = 10

    # lists of ids and the corners belonging to each id
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)

    # font for displaying text (below)
    font = cv2.FONT_HERSHEY_SIMPLEX

    # check if the ids list is not empty
    # if no check is added the code will crash
    if np.all(ids != None):

        # estimate pose of each marker and return the values
        # rvet and tvec-different from camera coefficients
        rvec, tvec ,_ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)
        #(rvec-tvec).any() # get rid of that nasty numpy value array error

        for i in range(0, ids.size):
            # draw axis for the aruco markers
            aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1)

        # draw a square around the markers
        aruco.drawDetectedMarkers(frame, corners)

        #corners[0][0][0-3 represent which corner of the aruco][0-x or 1-y axis value]
        top_left = corners[0][0][0][0]
        top_right = corners[0][0][1][0]
        bottom_right = corners[0][0][2][0]
        bottom_left = corners[0][0][3][0]
        width = top_right - top_left

        print("Top left " + str(top_left))
        print("Bottom left " + str(bottom_left))
        print("Bottom right " + str(bottom_right))
        print("Top right " + str(top_right))
        print("Aruco width: " + str(width))
        print("")

        pwm.start(100)

        print("\nTurning motor on\n")
        GPIO.output(Motor1A, GPIO.HIGH)
        GPIO.output(Motor1B, GPIO.LOW)
        GPIO.output(Motor1E, GPIO.HIGH)

        GPIO.output(Motor2A, GPIO.HIGH)
        GPIO.output(Motor2B, GPIO.LOW)
        GPIO.output(Motor2E, GPIO.HIGH)

        # code to show ids of the marker found
        strg = ''
        for i in range(0, ids.size):
            strg += str(ids[i][0])+', '

        cv2.putText(frame, "Id: " + strg, (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA)

    else:
        # code to show 'No Ids' when no markers are found
        cv2.putText(frame, "No Ids", (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
        GPIO.output(Motor1E, GPIO.LOW)
        GPIO.output(Motor2E, GPIO.LOW)

    # display the resulting frame
    cv2.imshow('frame', frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()

Приведенный выше сценарий позволяет останавливать Motor1E, а не Motor2E. При устранении неполадок я понял, что поменял местами эти две строки: 

pwm = GPIO.PWM(22, 100)
pwm = GPIO.PWM(23, 100)

на 

pwm = GPIO.PWM(23, 100)
pwm = GPIO.PWM(22, 100)

Произойдет обратное (Motor2E останавливается, а другой продолжает вращаться). Я предполагаю, что это очень небольшая ошибка в моем скрипте или может быть конфликт с циклами. Прошу прощения, если это кажется небольшой ошибкой, но я ценю любую помощь, спасибо!

...