Multiprocessing PID控制器控制伺服系统初始角度的设定

我遵循这一点，并添加了扫描程序。一旦检测到对象，扫描循环将中断，并从教程中恢复到跟踪模式。问题在于，轨迹模式有一个默认位置，当它被激活时，该位置通常位于扫描期间对象所在的FOV之外

伺服角度由PID控制器更新。因此，我需要找到一种方法，将初始值设置为跟踪模式激活前扫描伺服的最后一个角度，然后从PID控制器更新以跟踪对象。我在教程存储库中使用的主要文档是manager.py：

import logging
from multiprocessing import Value, Process, Manager, Queue

import pantilthat as pth
import signal
import sys
import time
import RPi.GPIO as GPIO

from rpi_deep_pantilt.detect.util.visualization import visualize_boxes_and_labels_on_image_array
from rpi_deep_pantilt.detect.camera import run_pantilt_detect
from rpi_deep_pantilt.control.pid import PIDController

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(8,GPIO.OUT)

logging.basicConfig()
LOGLEVEL = logging.getLogger().getEffectiveLevel()

RESOLUTION = (320, 320)

SERVO_MIN = -90
SERVO_MAX = 90

CENTER = (
    RESOLUTION[0] // 2,
    RESOLUTION[1] // 2
)


# function to handle keyboard interrupt
def signal_handler(sig, frame):
    # print a status message
    print("[INFO] You pressed `ctrl + c`! Exiting...")

    # disable the servos
    pth.servo_enable(1, False)
    pth.servo_enable(2, False)
    GPIO.output(8,GPIO.LOW)

    # exit
    sys.exit()

def in_range(val, start, end):
    # determine the input value is in the supplied range
    return (val >= start and val <= end)


def set_servos(pan, tilt, scan):
    # signal trap to handle keyboard interrupt
    signal.signal(signal.SIGINT, signal_handler)
    
    pn = 90
    tt = 25
    
    while scan.value == 't':     
        print('Scanning')
            
        pth.pan(pn)
        pth.tilt(tt)
        
        pn = pn-1
        
        if pn <= -90:
            pn = 90
        
        time.sleep(0.1)
        
        continue
        
    pan.value = -1*pn
    tilt.value = tt
    
    g=0
    
            
    while True:
#        if g<1:
#            pan.value = -1*pn
#            tilt.value = tt
        
        pan_angle = -1 * pan.value
        tilt_angle = tilt.value
        
        if g<6:
            print(pan_angle)
            print(tilt_angle)
        g=g+1
        
        # if the pan angle is within the range, pan
        if in_range(pan_angle, SERVO_MIN, SERVO_MAX):
            pth.pan(pan_angle)
        else:
            logging.info(f'pan_angle not in range {pan_angle}')

        if in_range(tilt_angle, SERVO_MIN, SERVO_MAX):
            pth.tilt(tilt_angle)
        else:
            logging.info(f'tilt_angle not in range {tilt_angle}')
            
 
    
    
def pid_process(output, p, i, d, box_coord, origin_coord, action):
    # signal trap to handle keyboard interrupt
    signal.signal(signal.SIGINT, signal_handler)

    # create a PID and initialize it
    p = PIDController(p.value, i.value, d.value)
    p.reset()
    

    # loop indefinitely
    while True:
        error = origin_coord - box_coord.value
        output.value = p.update(error)
        # logging.info(f'{action} error {error} angle: {output.value}')
    

def pantilt_process_manager(
    model_cls,
    labels=('Raspi',),
    rotation=0
):
    
    pth.servo_enable(1, True)
    pth.servo_enable(2, True)
    with Manager() as manager:

        
        scan = manager.Value('c', 't')
        
        # set initial bounding box (x, y)-coordinates to center of frame
        center_x = manager.Value('i', 0)
        center_y = manager.Value('i', 0)

        center_x.value = RESOLUTION[0] // 2
        center_y.value = RESOLUTION[1] // 2
        

        # pan and tilt angles updated by independent PID processes
        pan = manager.Value('i', 0)
        tilt = manager.Value('i', 0)

        # PID gains for panning
        pan_p = manager.Value('f', 0.05)
        # 0 time integral gain until inferencing is faster than ~50ms
        pan_i = manager.Value('f', 0.1)
        pan_d = manager.Value('f', 0)

        # PID gains for tilting
        tilt_p = manager.Value('f', 0.15)
        # 0 time integral gain until inferencing is faster than ~50ms
        tilt_i = manager.Value('f', 0.2)
        tilt_d = manager.Value('f', 0)

        detect_processr = Process(target=run_pantilt_detect,
                                  args=(center_x, center_y, labels, model_cls, rotation, scan))

        pan_process = Process(target=pid_process,
                              args=(pan, pan_p, pan_i, pan_d, center_x, CENTER[0], 'pan'))

        tilt_process = Process(target=pid_process,
                               args=(tilt, tilt_p, tilt_i, tilt_d, center_y, CENTER[1], 'tilt'))

        servo_process = Process(target=set_servos, args=(pan, tilt, scan))
        
        
        detect_processr.start()
        pan_process.start()
        tilt_process.start()
        servo_process.start()
        
        detect_processr.join()
        pan_process.join()
        tilt_process.join()
        servo_process.join()

if __name__ == '__main__':
    pantilt_process_manager()

这是PID脚本，对我来说是胡言乱语：

# import necessary packages
import time


class PIDController:
    def __init__(self, kP=1, kI=0, kD=0):

        # initialize gains
        self.kP = kP
        self.kI = kI
        self.kD = kD

    def reset(self):
        # intialize the current and previous time
        self.time_curr = time.time()
        self.time_prev = self.time_curr

        # initialize the previous error
        self.error_prev = 0

        # initialize the term result variables
        self.cP = 0
        self.cI = 0
        self.cD = 0

    def update(self, error, sleep=0.01):

        time.sleep(sleep)
        # grab the current time and calculate delta time / error
        self.time_curr = time.time()
        time_delta = self.time_curr - self.time_prev
        error_delta = error - self.error_prev

        # proportional term
        self.cP = error

        # integral term
        self.cI += error * time_delta

        # derivative term and prevent divide by zero
        self.cD = (error_delta / time_delta) if time_delta > 0 else 0

        # save previous time and error for the next update
        self.time_prev = self.time_curr
        self.error_prev = error

        # sum the terms and return
        return sum([
            self.kP * self.cP,
            self.kI * self.cI,
            self.kD * self.cD]
        )

有趣的一点是，一旦一个物体被跟踪，它被摄像机丢失，它不会恢复到任何位置，它会停留在原来的位置，直到它再次检测到。所以，我不明白为什么传递初始p/t值不会让它停留在那个位置，直到它再次拾取对象

# import necessary packages
import time


class PIDController:
    def __init__(self, kP=1, kI=0, kD=0):

        # initialize gains
        self.kP = kP
        self.kI = kI
        self.kD = kD

    def reset(self):
        # intialize the current and previous time
        self.time_curr = time.time()
        self.time_prev = self.time_curr

        # initialize the previous error
        self.error_prev = 0

        # initialize the term result variables
        self.cP = 0
        self.cI = 0
        self.cD = 0

    def update(self, error, sleep=0.01):

        time.sleep(sleep)
        # grab the current time and calculate delta time / error
        self.time_curr = time.time()
        time_delta = self.time_curr - self.time_prev
        error_delta = error - self.error_prev

        # proportional term
        self.cP = error

        # integral term
        self.cI += error * time_delta

        # derivative term and prevent divide by zero
        self.cD = (error_delta / time_delta) if time_delta > 0 else 0

        # save previous time and error for the next update
        self.time_prev = self.time_curr
        self.error_prev = error

        # sum the terms and return
        return sum([
            self.kP * self.cP,
            self.kI * self.cI,
            self.kD * self.cD]
        )