Voiture de patrouille de ligne

Tutoriel vidéo 20 - Voiture de patrouille de ligne : https://singtown.com/learn/50037/

Cet exemple montre la régression linéaire du retour sur investissement à l'aide de la méthode get_regression() sur OpenMV Cam. En utilisant cette méthode, il est facile de demander au robot de suivre toutes les lignes pointant dans la même direction générale.

Cette routine peut être utilisée pour la patrouille de ligne de robots et l'effet est très bon.

La voiture qui court après le ballon

Tutoriel vidéo 9 - Une voiture qui court après le ballon : https://singtown.com/learn/49239/

Préparer le matériel

• Carte OpenMV x1 :\

• Plancher de voiture imprimé en 3D :

• Batterie au lithium 3.7 V x1 :

• Carte pilote de moteur TB6612 x1 :

• Roue Oeil de Bull x2 :

• Moteur N20 DC x2 (y compris base fixe, pneus inclus) :

• Écrou à vis M3*20 x2 :

• Écrou à vis M3*20 x2

Connectez le circuit et testez le moteur

Écrire le module de la voiture

Tout d’abord, nous devons répondre : pourquoi avons-nous besoin d’écrire des modules ? Il n'est pas difficile de piloter directement le moteur. – Le code étant ainsi réutilisable au maximum, la logique de commande est indépendante de la structure de la voiture. Pour différentes voitures, changez simplement le module de la voiture.

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

Enregistrez le fichier ci-dessus sous car.py et, selon Utilisation du module, enregistrez car.py sur OpenMV.

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

import car

while True:
    car.run(100,100)

Vérifiez si la voiture avance. Sinon, modifiez l'inverse_gauche et l'inverse_droite des deuxième et troisième lignes pour inverser les roues gauche ou droite afin de vous assurer que la voiture avance.

Implémentation de l'algorithme PID

L'algorithme pid est un algorithme largement utilisé en contrôle, et il existe de nombreux principes sur Internet.\ https://zh.wikipedia.org/wiki/PID Contrôleur\ http://baike.baidu.com/link?url=-obQq78Ur4bTeqA10bIniO6y0euQFcWL9WW18vq2hA3fyHN3rt32o79F2WPE7cK0Di9M6904rlHD9ttvVTySIK\ Le code est encore très simple. J'ai directement copié le code source d'une commande 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')

Également selon Module Usage, enregistrez pid.py sur OpenMV.

Ajustez les paramètres pour obtenir les résultats suivants

L'essentiel est d'ajuster les deux paramètres de PI, 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())

Si vous souhaitez créer une voiture qui suit la ligne, il vous suffit d'utiliser la valeur de retour thêta et rho de l'objet ligne obtenu par ce programme (thêta représente l'angle du segment de ligne renvoyé, rho représente la distance de décalage), et d'utiliser thêta et rho pour contrôler l'angle de la voiture.

rho est plus important. Si vous n'utilisez pas thêta, vous pouvez simplement utiliser rho.

Rendu d'opération :