Voiture suiveuse de ligne
Tutoriel vidéo 20 - Voiture suiveuse de ligne : https://singtown.com/learn/50037/
Cet exemple montre l'utilisation de la méthode get_regression() sur l'OpenMV Cam pour obtenir la régression linéaire de la ROI. Grâce à cette méthode, il est facile de faire suivre au robot toutes les lignes pointant dans la même direction générale.
Cette routine peut être utilisée pour le suivi de ligne par un robot, et l'effet est très bon.
Voiture chassant des balles
Tutoriel vidéo 9 - La voiture qui chasse la balle : https://singtown.com/learn/49239/
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.
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
THRESHOLD = (5, 70, -23, 15, -57, 0) # Seuil en niveaux de gris pour les objets sombres...
import csi, image, time
csi0 = csi.CSI()
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()
csi0.reset()
csi0.vflip(True)
csi0.hmirror(True)
csi0.pixformat(csi.RGB565)
csi0.framesize(csi.QQQVGA) # 80x60 (4 800 pixels) - O(N^2) max = 2 304 0000.
#csi0.window([0,20,80,40])
csi0.snapshot(time = 2000) # ATTENTION : si vous utilisez QQVGA, cela peut parfois
clock = time.clock() # prendre plusieurs secondes pour traiter une image.
while(True):
clock.tick()
img = csi0.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, 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 voulez fabriquer une voiture suiveuse de ligne, il suffit d'utiliser les valeurs de retour theta et rho de l'objet ligne obtenu par ce programme (theta représente l'angle du segment de ligne renvoyé, rho représente la distance de décalage), et d'utiliser theta et rho pour contrôler l'angle de la voiture.
Rho est plus important. Si vous n'utilisez pas theta, vous pouvez simplement utiliser rho.
Schéma de l'effet en fonctionnement :
