能帮忙看看为什么小车无法巡直线吗?老是跑偏。(轨道只是直线,没有弯道)
-
THRESHOLD = (12, 100, -14, 117, -116, 78) # 红色阈值
import sensor, image, time, math
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() #打开openmv的RGB灯用来补光,为了避免颜色受环境的影响
#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
sensor.set_auto_gain(False) #关闭自动增益
sensor.set_auto_whitebal(False) # 在对颜色识别时,关闭白平衡
clock = time.clock() # to process a frame sometimes.#补充find_apriltags()函数参数
#由Tx、Ty、Tz三个方向确定镜头与tag标签的实际距离
#比例系数k =实际/根号(TxTx +TyTy +Tz*Tz)
#经过多次实距测量与虚距计算,求得平均比例系数K=24.5while(True):
clock.tick()
img = sensor.snapshot().binary([THRESHOLD]) #img.binary进行图像的阈值分割。黑白。
line = img.get_regression([(100,100,0,0,0,0)], robust = True)
if (line): #此函数还会返回一个line对象,即视野中一条直线
rho_err = abs(line.rho())-img.width()/2 #利用line对象的theta返回值和rho,(theta代表返回线段的角度, rho代表偏移的距离),利用theta和rho来控制小车角度
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: #magnitude越大,说明线性回归效果好。再进行下面的pid运算
#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 #将两个pid参数相加,output输出的参数是什么?
car.run(30+output, 30-output)
else:
car.run(0,0)
else:
car.run(0,-0) #~~~~这个位置感觉不应该原地转,当小车向前行驶,停下run(0,0)时要识别tag,而且不能再车位中转
#pass
#补充,现在的问题是,小车以黑白巡线了,但巡线后不能识别tag(tag是彩图),下面两行代码是我的处理。
sensor.set_pixformat(sensor.RGB565) #更改相机像素模式
img = sensor.snapshot() #截取图像
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # for循环,tag相当于变量i;不说默认为寻找TAG36H11
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation())
#遇到tag时电机启停
Tx=tag.x_translation()
Ty=tag.y_translation()
Tz=tag.z_translation()
d=24.5math.sqrt(TxTx+TyTy+TzTz)
if d>9: #当实距大于9,小车开
car.run(30,30)
elif d > 6 and d <9: #实距6~9范围内,小车停
car.run(0,0)
else: #其他距离,小车停
car.run(0,0)
print("Tx: %f, Ty %f, Tz %f" % print_args)
#print(clock.fps())
-
https://singtown.com/learn/50037/
如果是不一样的小车,没有硬件我无法帮助你。