Linienstreifenwagen

Video-Tutorial zum Streifenwagen mit 20 Reihen: https://singtown.com/learn/50037/

Dieses Beispiel zeigt eine lineare Regression mit der Methode get_regression() auf OpenMV Cam, um den ROI zu ermitteln. Mit dieser Methode ist es einfach, den Roboter alle Linien verfolgen zu lassen, die in die gleiche allgemeine Richtung zeigen.

Diese Routine kann für die Roboterlinienpatrouille verwendet werden und hat einen sehr guten Effekt.

Das Auto jagt den Ball

Video-Tutorial: 9 Autos jagen dem Ball hinterher: https://singtown.com/learn/49239/

Bereiten Sie Materialien vor

OpenMV-Board x1:\
3D-gedruckter Autoboden:

3,7 V Lithiumbatterie x1:

TB6612 Motortreiberplatine x1:

Bullaugenrad x2:

N20-Gleichstrommotor x2 (einschließlich fester Basis, einschließlich Reifen):

M3*20 Schraubenmutter x2:

M2*4 selbstschneidende Schrauben x2

Schließen Sie den Stromkreis an und testen Sie den Motor

Schreiben Sie das Modul des Autos

Zunächst möchte ich antworten: Warum müssen Sie ein Modul schreiben? Es ist nicht schwierig, den Motor direkt anzutreiben. Da dieser Code die beste Wiederverwendbarkeit aufweist, ist die Steuerlogik unabhängig von der Struktur des Fahrzeugs. Bei verschiedenen Fahrzeugen wechseln Sie einfach das Modul des Fahrzeugs.

car.py

from pyb import Pin, Timer
inverse_left=False  #change it to True to inverse left wheel
inverse_right=False #change it to True to inverse right wheel

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)))

Speichern Sie die obige Datei als car.py und speichern Sie car.py gemäß [Modulverwendung] (/MCU/module.md) in OpenMV.

Testen Sie den Code in der IDE:\ main.py

import car

while True:
    car.run(100,100)

Überprüfen Sie, ob sich das Auto vorwärts bewegt. Wenn nicht, ändern Sie in der zweiten und dritten Zeile die Werte „inverse_left“ und „inverse_right“, um das linke oder rechte Rad umzukehren, um sicherzustellen, dass sich das Auto vorwärts bewegt.

Implementierung des PID-Algorithmus

Der PID-Algorithmus ist ein Algorithmus, der in der Steuerung weit verbreitet ist, und es gibt viele Prinzipien im Internet.\ https://zh.wikipedia.org/wiki/PID-Regler\ http://baike.baidu.com/link?url=-obQq78Ur4bTeqA10bIniO6y0euQFcWL9WW18vq2hA3fyHN3rt32o79F2WPE7cK0Di9M6904rlHD9ttvVTySIK\ Der Code ist immer noch sehr einfach. Ich habe den Quellcode einer Flugsteuerung direkt kopiert:\ https://github.com/wagnerc4/flight_controller/blob/master/pid.py\ Es ist eine Kopie von 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')

Speichern Sie pid.py auch gemäß Modulnutzung in OpenMV.

Passen Sie die Parameter an, um Folgendes zu erreichen

Die Hauptsache besteht darin, die beiden Parameter von PI anzupassen, http://blog.csdn.net/zyboy2000/article/details/9418257

THRESHOLD = (5, 70, -23, 15, -57, 0) # Grayscale threshold for dark things...
import sensor, image, time
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()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.

while(True):
    clock.tick()
    img = sensor.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.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())

Wenn Sie ein linienfolgendes Auto erstellen möchten, müssen Sie nur den Theta-Rückgabewert und Rho des von diesem Programm erhaltenen Linienobjekts verwenden (Theta stellt den Winkel des zurückgegebenen Liniensegments dar, Rho stellt den Versatzabstand dar) und verwenden Theta und Rho zur Steuerung des Fahrzeugwinkels.

Rho ist wichtiger. Wenn Sie kein Theta verwenden, können Sie einfach Rho verwenden.

Operationsrendering:

Streifenwagen