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/
Tutoriel vidéo 21 - Une voiture poursuivant d'autres objets : https://singtown.com/learn/50041/

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.

car.run(left_speed, right_speed) a 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, il tournera vers l'avant. S'il s'agit d'un nombre négatif, il tournera vers l'arrière. Plus le nombre est grand entre 0 et 100, plus la vitesse est grande.

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

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

results matching ""

    No results matching ""