Voiture poursuivant une petite balle

Tutoriel vidéo 9 - La voiture qui chasse la balle : https://singtown.com/learn/49239/

Tutoriel vidéo 21 - Une voiture chassant d'autres objets : https://singtown.com/learn/50041/

Préparer le matériel

• Carte OpenMV x1 :\

• Base de voiture imprimée en 3D :

• Batterie au lithium 3,7V x1 :

• Carte de pilotage moteur TB6612 x1 :

• Roue folle (bull's eye) x2 :

• Moteur DC N20 x2 (avec support fixe, avec pneu) :

• Écrou de vis M3*20 x2 :

• Vis autotaraudeuse M2*4 x2

Connecter le circuit et tester le moteur

Écrire le module de la voiture

Tout d'abord, pourquoi devons-nous écrire un module ? Il n'est pas difficile de piloter directement le moteur. – Parce que c'est la meilleure façon de réutiliser le code, et la logique de contrôle est indépendante de la structure de la voiture. Pour différentes voitures, il suffit de changer le module de la voiture.

car.py

from pyb import Pin, Timer
inverse_left=False  # changez en True pour inverser la roue gauche
inverse_right=False # changez en True pour inverser la roue droite

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

Enregistrez le fichier ci-dessus sous car.py, et enregistrez car.py sur OpenMV selon l'utilisation des modules.

Testez le code dans l'IDE :\ main.py

import car

while True:
    car.run(100,100)

Vérifiez si la voiture avance. Sinon, modifiez inverse_left et inverse_right aux deuxième et troisième lignes pour inverser la roue gauche ou droite afin de garantir que la voiture avance.

car.run(left_speed, right_speed) prend deux paramètres, l'un est la vitesse de la roue gauche et l'autre est la vitesse de la roue droite.

Si le paramètre de vitesse est un nombre positif, la roue tournera vers l'avant, s'il est négatif, elle tournera vers l'arrière. Plus le nombre est grand entre 0 et 100, plus ce sera rapide.

Implémentation de l'algorithme PID

L'algorithme PID est un algorithme très courant utilisé en contrôle, et il existe de nombreux principes sur Internet.\ https://zh.wikipedia.org/wiki/PID控制器\ http://baike.baidu.com/link?url=-obQq78Ur4bTeqA10bIniO6y0euQFcWL9WW18vq2hA3fyHN3rt32o79F2WPE7cK0Di9M6904rlHD9ttvVTySIK\ Le code est très simple. J'ai directement copié le code source d'un contrôleur de vol :\ https://github.com/wagnerc4/flight_controller/blob/master/pid.py\ C'est une copie d'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')

Enregistrez également pid.py sur OpenMV selon l'utilisation des modules.

Ajuster les paramètres pour réaliser le suivi

L'essentiel est d'ajuster les deux paramètres de PI, http://blog.csdn.net/zyboy2000/article/details/9418257

# Exemple de détection de blob
#
# Cet exemple montre comment utiliser la fonction find_blobs pour trouver des
# blobs de couleur dans l'image. Cet exemple recherche en particulier des
# objets vert foncé.

import csi, image, time
import car
csi0 = csi.CSI()

from pid import PID

# Vous devrez peut-être ajuster les réglages ci-dessus pour suivre des objets verts...
# Sélectionnez une zone dans le tampon d'image pour copier les réglages de couleur.

csi0.reset() # Initialise le capteur de la caméra.
csi0.pixformat(csi.RGB565) # utilise RGB565.
csi0.framesize(csi.QQVGA) # utilise QQVGA pour la vitesse.
csi0.snapshot(frames=10) # Laisse les nouveaux réglages prendre effet.
csi0.auto_whitebal(False) # désactive ceci.
clock = time.clock() # Suit le FPS.

# Pour que le suivi de couleur fonctionne vraiment bien, vous devriez idéalement
# être dans un environnement très, très, très contrôlé où l'éclairage est 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() # Suit les millisecondes écoulées entre les snapshot().
    img = csi0.snapshot() # Prend une photo et renvoie l'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:
            # Dessine un rectangle autour du 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)