Das Auto jagt den Ball

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

Video-Tutorial 21 – Auto jagt andere Objekte: https://singtown.com/learn/50041/

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.

car.run(left_speed, right_speed) hat zwei Parameter, einer ist die Geschwindigkeit des linken Rades und der andere ist die Geschwindigkeit des rechten Rades.

Wenn der Geschwindigkeitsparameter eine positive Zahl ist, dreht er sich vorwärts. Wenn er eine negative Zahl ist, dreht er sich rückwärts. Je größer die Zahl zwischen 0 und 100, desto höher ist die Geschwindigkeit.

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

# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car
from pid import PID

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
green_threshold   = (76, 96, -110, -30, 8, 66)
size_threshold = 2000
x_pid = PID(p=0.5, i=1, imax=100)
h_pid = PID(p=0.05, i=0.1, imax=50)

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([green_threshold])
    if blobs:
        max_blob = find_max(blobs)
        x_error = max_blob[5]-img.width()/2
        h_error = max_blob[2]*max_blob[3]-size_threshold
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        img.draw_rectangle(max_blob[0:4]) # rect
        img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
        x_output=x_pid.get_pid(x_error,1)
        h_output=h_pid.get_pid(h_error,1)
        print("h_output",h_output)
        car.run(-h_output-x_output,-h_output+x_output)
    else:
        car.run(18,-18)