why does my PID controller cant track an object with image processing?

Question

I'm programming a robot to navigate the room using a camera to read off signs (such as: Bathroom-Right etc.), I'm using an Alphabot2 kit and an RPI3B+

the robot starts in front of the first sign, and each turn is 90 degrees.

since the robot should basically move in a fairly straight line, I'm not sure what I am doing wrong. I can't get the robot to reach the first sign, he just loses control.

I wrote a PID controller and tweaked the parameters for that task but it didn't seem to help. will appreciate your input

It seems that the robot moves way to fast for the pid controller

import RPi.GPIO as GPIO
import cv2 as cv
import numpy as np
import pytesseract
import time
from picamera.array import PiRGBArray
from picamera import PiCamera

#MESURED PARAMETERS#
focalFactor =1650.83
knownWidth =18.4 #cm
iPWM = 20 # initial motor power in duty cycle
MAX_PWM = 100 #
dt = 0.001 # time step

#PID PARAMETERS#
KP = 0.12
KD = 0.01
KI = 0.00005

TARGETS = ['Restroom', 'Restaurant', 'Balcony']
TARGET = 'Restroom'


class PID(object):
    def __init__(self,target):
        
        self.target = target
        self.kp = KP
        self.ki = KI
        self.kd = KD 
        self.setpoint = 320
        self.error = 0
        self.integral_error = 0
        self.error_last = 0
        self.derivative_error = 0
        self.output = 0
        
    def compute(self, pos):
        self.error = self.setpoint - pos
        self.derivative_error = (self.error - self.error_last) / dt
        self.error_last = self.error
        self.output = self.kp*self.error + self.ki*self.integral_error + self.kd*self.derivative_error
        if(abs(self.output)>= MAX_PWM-iPWM and (((self.error>=0) and (self.integral_error>=0))or((self.error<0) and (self.integral_error<0)))):
            #no integration
            self.integral_error = self.integral_error
        else:
            #rectangular integration
            self.integral_error += self.error * dt
        
        if self.output>= MAX_PWM-iPWM:
            self.output = iPWM
            
        elif self.output <= -MAX_PWM:
            self.output = -iPWM 
        print("output"+ str(self.output))
        return self.output




class MOTORS(object):
    
    def __init__(self,ain1=12,ain2=13,ena=6,bin1=20,bin2=21,enb=26):
        self.AIN1 = ain1
        self.AIN2 = ain2
        self.BIN1 = bin1
        self.BIN2 = bin2
        self.ENA = ena
        self.ENB = enb
        self.PA  = iPWM
        self.PB  = iPWM

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.AIN1,GPIO.OUT)
        GPIO.setup(self.AIN2,GPIO.OUT)
        GPIO.setup(self.BIN1,GPIO.OUT)
        GPIO.setup(self.BIN2,GPIO.OUT)
        GPIO.setup(self.ENA,GPIO.OUT)
        GPIO.setup(self.ENB,GPIO.OUT)
        self.PWMA = GPIO.PWM(self.ENA,100)
        self.PWMB = GPIO.PWM(self.ENB,100)
        self.PWMA.start(self.PA)
        self.PWMB.start(self.PB)
        self.stop()

    def forward(self):
        self.PWMA.ChangeDutyCycle(self.PA)
        self.PWMB.ChangeDutyCycle(self.PB)
        GPIO.output(self.AIN1,GPIO.LOW)
        GPIO.output(self.AIN2,GPIO.HIGH)
        GPIO.output(self.BIN1,GPIO.LOW)
        GPIO.output(self.BIN2,GPIO.HIGH)
    
    def updatePWM(self,value):
        if value<0:
            
            self.PB = iPWM
            if iPWM+abs(value)<MAX_PWM:
                self.PA = iPWM+abs(value)
            else:
                self.PA = iPWM
            print('PA =' +str(self.PA))
            print('PB =' +str(self.PB))
            
        elif value>0:
            self.PA = iPWM
            if iPWM+value<MAX_PWM:
                self.PB = iPWM+value
            else:
                self.PB = iPWM
            print('PA =' +str(self.PA))
            print('PB =' +str(self.PB))

        else:
            self.PA = iPWM
            self.PB = iPWM
        

        self.PWMA.ChangeDutyCycle(self.PA)
        self.PWMB.ChangeDutyCycle(self.PB)    
        GPIO.output(self.AIN1,GPIO.LOW)
        GPIO.output(self.AIN2,GPIO.HIGH)
        GPIO.output(self.BIN1,GPIO.LOW)
        GPIO.output(self.BIN2,GPIO.HIGH)

    
    def stop(self):
        self.PWMA.ChangeDutyCycle(0)
        self.PWMB.ChangeDutyCycle(0)
        GPIO.output(self.AIN1,GPIO.LOW)
        GPIO.output(self.AIN2,GPIO.LOW)
        GPIO.output(self.BIN1,GPIO.LOW)
        GPIO.output(self.BIN2,GPIO.LOW)
        
def getDistance(knownWidth, focalLength, perWidth):
    
    # compute and return the distance from the maker to the camera
    return (knownWidth * focalFactor) / perWidth

def cropImage(Image, XY, WH):
    
    (x, y) = XY
    (w, h) = WH
    crop = Image[y:y + h, x:x + w]
    return crop

camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 30

rawCapture = PiRGBArray(camera, size=(640, 480))


mot = MOTORS()
pid = PID(TARGET)
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
    img = frame.array
    imgHSV = cv.cvtColor(img,cv.COLOR_BGR2HSV)
    h_min = 31
    h_max = 42
    s_min = 50
    s_max = 214
    v_min = 108
    v_max = 255

    
    lower = np.array([h_min,s_min,v_min])
    upper = np.array([h_max,s_max,v_max])
    mask = cv.inRange(imgHSV,lower,upper)
    result = cv.bitwise_and(img,img, mask = mask)
    
    
    
    #find contours
    contour = img.copy()
    # show the frame
    imgBlur = cv.GaussianBlur(result,(5,5),1)
    imgGray = cv.cvtColor(imgBlur,cv.COLOR_BGR2GRAY)
    threshold1 = 50
    threshold2 =200
    imgCanny = cv.Canny(imgGray,threshold1,threshold2)
    kernel = np.ones((5,5))
    imgDil = cv.dilate(imgCanny,kernel, iterations=1)
    _,contours,hierarchy = cv.findContours(imgDil, cv.RETR_EXTERNAL,cv.CHAIN_APPROX_NONE)

    for cnt in contours:
        area = cv.contourArea(cnt)
        
        if area>10000:
            peri = cv.arcLength(cnt,True)
            approx = cv.approxPolyDP(cnt,0.02*peri,True)
            if len(approx) == 4:
                cv.drawContours(contour,cnt,-1,(255,0,255),3)
                
                print(area)
                x,y,w,h = cv.boundingRect(approx)
                d = getDistance(knownWidth, focalFactor, w)
                cv.rectangle(contour, (x,y),(x+w,y+h),(0,255,0),5)
                cv.putText(contour, str(d)+"meters"  ,(contour.shape[1] - 300, contour.shape[0] - 20), cv.FONT_HERSHEY_SIMPLEX,2.0, (0, 255, 0), 3)
                cx = int(x+w//2)
                cy = int(y+h//2)
                if d<= 70:
                    #read the sign
                    mot.stop()
#                     imgText= cropImage(contour,(x,y), (w,h))
#                     text = pytesseract.image_to_string(imgText)
#                     
#                     if count ==1:    
#                         sign1 = list(text.split("\n"))
#                         print(text)
#                         print(sign1)
#                         for line in sign1:
#                             if list(line.split("-"))[0] == pid.target:
#                                 command = list(line.split("-"))[1]
#                         if command == 'Right':
#                             print("RIGHT")
#                             
#                         if command == 'LEFT':
#                             print("LEFT")                             
#                         if command == 'STRAIGHT':
#                             print("STRAIGHT")                             
#                         
#                     if count ==2:    
#                         sign2 = list(text.split(" "))
#                         command = sign2[1]
#                         if command == 'RIGHT':
#                             print("RIGHT")
#                         if command == 'LEFT':
#                             print("LEFT")
#                             
                        
                dutyCycle = pid.compute(cx)
                pwm = mot.updatePWM(dutyCycle)
                print('output=' + str(dutyCycle))
                print("error:" + str(320-cx))
                cv.circle(contour,(cx,cy),5,(0,0,255,cv.FILLED))
                cv.circle(contour,(320,240),5,(255,255,0,cv.FILLED))

    
    stack = stackImages(0.7,([mask,result],[imgDil,contour]))
    cv.imshow("Stacked Images",stack)
    rawCapture.truncate(0)
    if cv.waitKey(1) & 0xFF == ord('q'):
        break
cv.destroyAllWindows()
camera.close()
pwmStop()
GPIO.cleanup()

So what exactly does it do? Does it oscillate or does it go towards one side? — KZiovas, Oct 05 '21 at 14:15
You have several things going on, so you need to first decompose and try to isolate in which aspect lies the problem. Does it recognize the signs? If so, what is the PID controller trying to achieve? ... BTW, `setpoint` is fixed internally and `target` is not used within the PID; this seems off to me. — ATony, Oct 05 '21 at 16:26
thanks for answering, it recognizes the sign, the problem lies in the PID control. as soon as the robot calculates the first error, he just starts going in cicrcles — ofer simchovitch, Oct 08 '21 at 07:50

why does my PID controller cant track an object with image processing?

0 Answers0