Coche de patrulla de línea

Tutorial en vídeo 20 - Coche de patrulla de línea: https://singtown.com/learn/50037/

Este ejemplo muestra el uso del método get_regression() en la OpenMV Cam para obtener la regresión lineal de la ROI. Con este método, es fácil hacer que el robot siga todas las líneas que apuntan en la misma dirección general.

Esta rutina se puede usar para la patrulla de líneas de un robot, y el efecto es muy bueno.

Coche que persigue pelotas

Tutorial en vídeo 9 - El coche que persigue la pelota: https://singtown.com/learn/49239/

Preparar materiales

  • Placa de circuito OpenMV x1:\

  • Base del coche impresa en 3D:

  • Batería de litio de 3.7V x1:

  • Placa controladora de motor TB6612 x1:

  • Rueda tipo ojo de buey x2:

  • Motor DC N20 x2 (incluye base fija, incluye neumático):

  • Tornillo y tuerca M3*20 x2:

  • Tornillo autorroscante M2*4 x2

Conectar el circuito y probar el motor

Escribir el módulo del coche

Antes que nada, ¿por qué necesitamos escribir un módulo? No es difícil manejar el motor directamente. – Porque esto ofrece la mejor reutilización de código, y la lógica de control es independiente de la estructura del coche. Para diferentes coches, basta con cambiar el módulo del coche.

car.py

from pyb import Pin, Timer
inverse_left=False  # cámbialo a True para invertir la rueda izquierda
inverse_right=False # cámbialo a True para invertir la rueda derecha

ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
bin1 =  Pin('P2', Pin.OUT_PP)
bin2 =  Pin('P3', Pin.OUT_PP)
ain1.low()
ain2.low()
bin1.low()
bin2.low()

pwma = Pin('P7')
pwmb = Pin('P8')
tim = Timer(4, freq=1000)
ch1 = tim.channel(1, Timer.PWM, pin=pwma)
ch2 = tim.channel(2, Timer.PWM, pin=pwmb)
ch1.pulse_width_percent(0)
ch2.pulse_width_percent(0)

def run(left_speed, right_speed):
    if inverse_left==True:
        left_speed=(-left_speed)
    if inverse_right==True:
        right_speed=(-right_speed)

    if left_speed < 0:
        ain1.low()
        ain2.high()
    else:
        ain1.high()
        ain2.low()
    ch1.pulse_width_percent(int(abs(left_speed)))

    if right_speed < 0:
        bin1.low()
        bin2.high()
    else:
        bin1.high()
        bin2.low()
    ch2.pulse_width_percent(int(abs(right_speed)))

Guarde el archivo anterior como car.py, y guarde car.py en OpenMV según el uso del módulo.

Pruebe el código en el IDE:\ main.py

import car

while True:
    car.run(100,100)

Compruebe si el coche se está moviendo hacia adelante. Si no es así, cambie inverse_left e inverse_right en la segunda y tercera línea para invertir la rueda izquierda o derecha, y así asegurarse de que el coche se mueva hacia adelante.

Implementación del algoritmo PID

El algoritmo PID es un algoritmo muy común utilizado en control, y hay muchos principios explicados en Internet.\ https://zh.wikipedia.org/wiki/PID控制器\ http://baike.baidu.com/link?url=-obQq78Ur4bTeqA10bIniO6y0euQFcWL9WW18vq2hA3fyHN3rt32o79F2WPE7cK0Di9M6904rlHD9ttvVTySIK\ El código es muy sencillo. Copié directamente el código fuente de un controlador de vuelo:\ https://github.com/wagnerc4/flight_controller/blob/master/pid.py\ Es una copia de ArduPilot\ https://github.com/ArduPilot/ardupilot

pid.py

from pyb import millis
from math import pi, isnan

class PID:
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._last_derivative = float('nan')

    def get_pid(self, error, scaler):
        tnow = millis()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1000:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt) / float(1000)
        output += error * self._kp
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                                     ((delta_time / (self._RC + delta_time)) * \
                                        (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative
        output *= scaler
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            if self._integrator < -self._imax: self._integrator = -self._imax
            elif self._integrator > self._imax: self._integrator = self._imax
            output += self._integrator
        return output
    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')

Guarde también pid.py en OpenMV según el uso del módulo.

Ajustar los parámetros para lograr el seguimiento

Lo principal es ajustar los dos parámetros de PI, http://blog.csdn.net/zyboy2000/article/details/9418257

THRESHOLD = (5, 70, -23, 15, -57, 0) # Umbral de escala de grises para objetos oscuros...
import csi, image, time
csi0 = csi.CSI()

from pyb import LED
import car
from pid import PID
rho_pid = PID(p=0.4, i=0)
theta_pid = PID(p=0.001, i=0)

LED(1).on()
LED(2).on()
LED(3).on()

csi0.reset()
csi0.vflip(True)
csi0.hmirror(True)
csi0.pixformat(csi.RGB565)
csi0.framesize(csi.QQQVGA) # 80x60 (4,800 píxeles) - O(N^2) máx = 2,3040,000.
#csi0.window([0,20,80,40])
csi0.snapshot(time = 2000)     # ADVERTENCIA: si usas QQVGA a veces puede tardar
clock = time.clock()                # varios segundos en procesar un fotograma.

while(True):
    clock.tick()
    img = csi0.snapshot().binary([THRESHOLD])
    line = img.get_regression([(100,100)], robust = True)
    if (line):
        rho_err = abs(line.rho)-img.width()/2
        if line.theta>90:
            theta_err = line.theta-180
        else:
            theta_err = line.theta
        img.draw_line(line, color = 127)
        print(rho_err,line.magnitude,rho_err)
        if line.magnitude>8:
            #if -40<b_err<40 and -30<t_err<30:
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            car.run(50+output, 50-output)
        else:
            car.run(0,0)
    else:
        car.run(50,-50)
        pass
    #print(clock.fps())

Si desea construir un coche de patrulla de línea, simplemente use los valores de retorno theta y rho del objeto de línea obtenido por este programa (theta representa el ángulo del segmento de línea devuelto, rho representa la distancia de desplazamiento), y use theta y rho para controlar el ángulo del coche.

Rho es más importante. Si no usa theta, puede usar solo rho.

Diagrama del efecto en funcionamiento:

results matching ""

    No results matching ""