Car chasing balls
Video tutorial 9 - The car that chases the ball: https://singtown.com/learn/49239/
Video Tutorial 21 - A car chasing other objects: https://singtown.com/learn/50041/
Prepare materials
OpenMV circuit board x1:\
3D printed car base:
- 3.7V lithium battery x1:
- TB6612 motor driver board x1:
- Bull's eye wheel x2:
- N20 DC motor x2 (including fixed seat, including tire):
- M3*20 screw nut x2:
- M2*4 self-tapping screw x2
Connect the circuit and test the motor
Write the module of the car
First of all, why do we need to write a module? It is not difficult to drive the motor directly. – Because this is the best code reusability, and the control logic is independent of the structure of the car. For different cars, just change the module of the car.
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)))
Save the above file as car.py, and save car.py to OpenMV according to the use of the module.
Test the code in the IDE:\ main.py
import car
while True:
car.run(100,100)
Check if the car is moving forward. If not, change inverse_left and inverse_right in the second and third lines to reverse the left or right wheel to ensure that the car is moving forward.
car.run(left_speed, right_speed) has two parameters, one is the speed of the left wheel and the other is the speed of the right wheel.
If the speed parameter is a positive number, it will turn forward, if it is a negative number, it will turn backward. The larger the number from 0 to 100, the faster it will be.
PID algorithm implementation
PID algorithm is a very common algorithm used in control, and there are many principles on the Internet.\ https://zh.wikipedia.org/wiki/PID控制器\ http://baike.baidu.com/link?url=-obQq78Ur4bTeqA10bIniO6y0euQFcWL9WW18vq2hA3fyHN3rt32o79F2WPE7cK0Di9M6904rlHD9ttvVTySIK\ The code is very simple. I directly copied the source code of a flight controller:\ https://github.com/wagnerc4/flight_controller/blob/master/pid.py\ It is a copy of 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')
Also save pid.py to OpenMV according to the use of the module.
Adjust parameters to achieve following
The main thing is to adjust the two parameters of 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)